編隊行駛系統(tǒng)中基于Kalman 濾波的相對定位算法研究

尹立強(qiáng),張鶴楊,霍俊臣,陳文浩,劉亞飛,朱文成

（河南科技學(xué)院信息工程學(xué)院,河南 新鄉(xiāng) 453003）

“編隊行駛”作為車聯(lián)網(wǎng)的三大主要應(yīng)用場景之一,也稱為“列隊行駛”,是指三輛及以上車輛,在同一路段排序行駛,后續(xù)車輛在頭部車輛的引領(lǐng)下,無需駕駛員操控便可自動行駛[1-3].車輛精確定位作為車聯(lián)網(wǎng)的核心技術(shù)之一,為車聯(lián)網(wǎng)編隊行駛應(yīng)用提供了最基本的位置信息.車聯(lián)網(wǎng)系統(tǒng)中車輛定位包含兩個方面：領(lǐng)航車輛定位和隨行車輛定位.領(lǐng)航車輛承擔(dān)著整個車隊的導(dǎo)航任務(wù),其定位過程要求具有絕對位置坐標(biāo),可以采用衛(wèi)星定位和基站定位聯(lián)合工作方式,彼此取長補(bǔ)短,提高定位精度.行駛過程中,隨行車輛跟隨領(lǐng)航車輛,領(lǐng)航車輛需要獲取隨行車輛的位置下達(dá)相應(yīng)的行駛指令,隨行車輛之間也需要保持一定的安全行駛距離,可以采用相對定位技術(shù)實現(xiàn)上述功能.隨行車輛之間采用相對定位技術(shù),不僅可以消除絕對定位初始化時間長、成本高的缺點,還可以有效減小定位誤差的累積,提高定位精度.本文著重分析探討編隊車輛之間的相對定位技術(shù).

1 編隊行駛原理

本系統(tǒng)采用leading-following 模式進(jìn)行編隊,每輛車在行駛過程中自動保持車間距離,并且?guī)ь^車輛無論是加減速、轉(zhuǎn)向還是剎車,跟隨的車輛都會實時同步完成.編隊行駛示意圖如圖1 所示.

圖1 編隊行駛示意圖Fig.1 Formation driving diagram

在編隊車輛行駛過程中,如果有其他車輛要求加入編隊或者有車輛要脫離編隊,首先向領(lǐng)航車輛發(fā)出申請,領(lǐng)航車輛同意后,以廣播形式通知其它隨行車輛,新加入車輛跟在編隊隊尾或者根據(jù)需要插入車隊中間,其它車輛要實時調(diào)整速度以保持安全距離和編隊隊形.本系統(tǒng)以領(lǐng)航車輛作為中央處理器,領(lǐng)航車輛安裝GPS 和車路協(xié)同感知系統(tǒng),領(lǐng)航車輛將自身定位信息和路面狀況及時以廣播形式通知隨行車輛.隨行車輛需將自身的相對測距信息告知領(lǐng)航車輛.車與車之間的通信采用C-V2V 技術(shù).系統(tǒng)原理框圖如圖2 所示.

圖2 系統(tǒng)整體原理框圖Fig.2 System overall principle block diagram

2 編隊行駛車輛定位方法

2.1 領(lǐng)航車輛定位

領(lǐng)航車輛安裝有GPS 和5G 通信模塊,因為領(lǐng)航車輛需要進(jìn)行行駛路徑導(dǎo)航,需要獲取領(lǐng)航車輛的絕對地理位置坐標(biāo),故此領(lǐng)航車輛可以采用GPS 和基站聯(lián)合定位方式.

2.1.1 GPS 定位GPS 定位系統(tǒng)主要由衛(wèi)星、控制部分、地面接收即用戶部分組成[4].GPS 系統(tǒng)共有24 顆衛(wèi)星, 每顆GPS 工作衛(wèi)星都發(fā)出用于導(dǎo)航定位的信號,GPS 用戶正是利用這些信號來進(jìn)行工作的.GPS的控制部分由分布在全球的由若干個跟蹤站所組成的監(jiān)控系統(tǒng)所構(gòu)成,根據(jù)其作用的不同,這些跟蹤站又被分為主控站、監(jiān)控站和注入站.GPS 的用戶部分由GPS 接收機(jī)、數(shù)據(jù)處理軟件及相應(yīng)的用戶設(shè)備如計算機(jī)氣象儀器等所組成.它的作用是接收GPS 衛(wèi)星所發(fā)出的信號,利用這些信號進(jìn)行導(dǎo)航定位等工作.

終端設(shè)備中的GPS 導(dǎo)航軟件能夠?qū)崟r提供游客所在位置的經(jīng)度、緯度與高程,定位精度小于20 m,并以標(biāo)準(zhǔn)形式顯示.GPS 系統(tǒng)能夠連續(xù)實時隱蔽低定位,一次定位時間僅幾秒到十幾秒,用戶無需發(fā)射任何電磁信號,只需接受衛(wèi)星導(dǎo)航信號即可定位,可全天候晝夜作業(yè),隱蔽性好.

GPS 定位方法有直接定位和差分定位兩種,差分定位又分為位置差分、偽距差分、載波相位差分.目前多采用偽距差分算法進(jìn)行定位.

2.1.2 基站定位 基站定位一般應(yīng)用于移動終端用戶,基站定位服務(wù)又叫做移動位置服務(wù)LBS（Location Based Service）,它是通過電信移動運營商的網(wǎng)絡(luò)獲取移動終端用戶的位置信息（經(jīng)、緯度坐標(biāo)）.移動終端測量不同基站的下行導(dǎo)頻信號,得到不同基站下行導(dǎo)頻的TOA（Time of Arrival,到達(dá)時刻）或TDOA（Time Difference of Arrival,到達(dá)時間差）,根據(jù)該測量結(jié)果并結(jié)合基站的坐標(biāo),一般采用三角公式估計算法,就能夠計算出移動終端的位置.實際的位置估計算法需要考慮多基站（3 個或3 個以上）定位的情況,因此算法要復(fù)雜很多.3GPP 的R16 版本中還提出了基于增強(qiáng)小區(qū)ID（E-CID）的定位方法[5],基站與終端互發(fā)參考信號測量Rx-Tx 時間差以得到RTT,確定終端所在的圓,結(jié)合DL-AoD（下行離開角）或UL-AoA（上行離開角）得到角度信息,它與圓的交點即為終端位置.這種定位方法的一個顯著的優(yōu)點是僅靠單站即可完成定位,不受基站之間同步精度的影響.一般而言,移動終端測量的基站數(shù)目越多,測量精度越高,定位性能改善越明顯.但是精度很大程度依賴于基站的分布及覆蓋范圍的大小,有時誤差會超過一公里.

隨著5G 網(wǎng)絡(luò)的廣泛覆蓋, 其定位精度也得到了很大提升.5G 相對于4G 在定位技術(shù)方面具備一些天然的優(yōu)勢,比如大帶寬（時間分辨率高）以及采用Massive MIMO 大規(guī)模天線技術(shù),具有更高分辨率的波束,可以實現(xiàn)更高精度的測距和測角,上述定位方法既可以單獨使用,也可以結(jié)合使用,以此來滿足不同場景的精度需求.

2.2 隨行車輛定位

在車聯(lián)網(wǎng)編隊行駛系統(tǒng)中,隨行車輛之間時刻需要保持合理的距離,車輛之間的距離定位尤為重要.目前,隨行車輛定位主要通過GPS、北斗等全球衛(wèi)星定位系統(tǒng)獲得絕對位置信息,然后利用位置坐標(biāo)計算車輛之間的距離.但是民用GPS 標(biāo)準(zhǔn)位置服務(wù)可提供10m 左右的定位精度[6],無法滿足編隊行駛的精確定位應(yīng)用要求,除此以外,在密集建筑群、森林、隧道、立交橋下、地下停車場等區(qū)域時,衛(wèi)星信號受到影響會造成較大的定位誤差,甚至定位失敗,所以基于衛(wèi)星定位的絕對坐標(biāo)定位方式就不適用.車輛定位和位置感知作為編隊行駛的必需條件, 其突出特點是車聯(lián)網(wǎng)編隊行駛系統(tǒng)只需要知道車輛間的相對距離而不需要車輛的絕對位置信息.為了克服衛(wèi)星定位存在的定位初始時間長、定位精度差、覆蓋受限等缺點,趙海濤等采用了偽距雙差的方式進(jìn)行車輛相對距離的測量[7],定位精度得到了提升,但是每輛車需要安裝較多傳感器,成本較高.

本系統(tǒng)著重研究隨行車輛定位方式,采用了一種不依賴于精確初始坐標(biāo)的車聯(lián)網(wǎng)相對定位技術(shù),隨行車輛安裝雷達(dá)傳感器,并采用Kalman 濾波算法對其測量過程進(jìn)行處理,該方法不僅可以有效減小定位誤差的累積效應(yīng),并且進(jìn)一步提高了定位精度,完善了車輛防碰撞機(jī)制,提高編隊車輛行駛的安全性.

3 編隊隨行車輛相對定位實現(xiàn)

3.1 雷達(dá)傳感器

雷達(dá)即Radar（Radio Detection and Ranging）,意為“無線電探測和測距”,即用無線電方法發(fā)現(xiàn)目標(biāo)并測定它們在空間的位置.典型的雷達(dá)系統(tǒng)主要由天線、收發(fā)轉(zhuǎn)換開關(guān)、發(fā)射機(jī)、接收機(jī)、定時器、顯示器、電源等部分組成[8].發(fā)射機(jī)在定時器控制下,產(chǎn)生高頻大功率的脈沖串,通過收發(fā)開關(guān)到達(dá)定向天線,以電磁波形式向外輻射.在天線控制設(shè)備的控制下,天線波束按照指定方向在空間掃描,電磁波在傳播過程中遇到目標(biāo)時,目標(biāo)受到激勵而產(chǎn)生二次輻射,二次輻射中的一小部分電磁波返回雷達(dá),為天線所收集,稱為回波信號.接收機(jī)將回波信號放大和變換后,送到顯示器上顯示,從而探測到目標(biāo)的存在.為了使雷達(dá)能夠在各個方向的廣闊空域內(nèi)搜索、發(fā)現(xiàn)和跟蹤目標(biāo),天線的定向波束以一定的方式在空間掃描.定時器用于控制雷達(dá)各個部分保持同步工作.收發(fā)轉(zhuǎn)換開關(guān)可使同一副天線兼作發(fā)射和接收之用.電源供給雷達(dá)各部分需要的電能.雷達(dá)工作原理如圖3 所示.

圖3 雷達(dá)工作原理圖Fig.3 Schematic diagram of radar operation

考慮到除了測距功能外,編隊行駛車輛還需要進(jìn)行車道識別、測速、測方位角度以及車身周邊的道路情況,本系統(tǒng)采用毫米波雷達(dá).毫米波雷達(dá)兼具有微波雷達(dá)和光電雷達(dá)的一些優(yōu)點,并且具有體積小、易集成和空間分辨率高的特點.車載毫米波雷達(dá)的工作頻率為一般為24 GHz 和77 GHz[9],可以同時對多個目標(biāo)進(jìn)行測距、測速以及方位測量.其測速是根據(jù)多普勒效應(yīng),而方位測量（包括水平角度和垂直角度）是通過天線的陣列方式來實現(xiàn)的.

3.2 TOA 測距方式

TOA 為基于電磁波空中傳播時間換算距離的點對點測距方法,算法易實現(xiàn),在成本和可靠性上優(yōu)勢顯著,因而受到廣泛關(guān)注.在TOA 點對點的距離測量方法中,典型的有單邊雙向測距（SS-TWR）、雙邊雙向測距（DS-TWR）方法[10-11].SS-TWR 是由設(shè)備發(fā)送一條消息并接收應(yīng)答,根據(jù)電磁波傳播時間換算成距離的測量方法.SS-TWR 測距方法中,交互信息幀少,總響應(yīng)時間短,錯誤幀數(shù)量少,傳輸過程穩(wěn)定.劉鵬媛[12]運用SS-TWR 對高速彈丸測定距離,仿真結(jié)果顯示測距誤差在40~60 cm.TOA 單邊雙向測距原理如圖4 所示.

圖4 TOA 單邊雙向測距原理圖Fig.4 TOA unilateral bidirectional ranging schematic diagram

根據(jù)發(fā)送設(shè)備發(fā)送“詢問”幀與收到“應(yīng)答”幀的時間差,即可計算出發(fā)送設(shè)備和應(yīng)答設(shè)備之間的距離.設(shè)發(fā)送設(shè)備發(fā)送“詢問”幀的時刻為ts,收到“應(yīng)答”幀的時刻為tr,電磁波傳播速度為c（3×108m/s）,則發(fā)送設(shè)備和接收設(shè)備之間的距離為：

3.3 隨行車輛測距模型

本系統(tǒng)中編隊車輛采用相對定位技術(shù),每輛隨行車輛均在車頭與車尾安裝測距模塊,通過測量車頭與相鄰前車車尾之間的距離確定自身的相對位置,并將定位信息通過通信模塊傳輸給領(lǐng)航車輛,原理圖如圖5 所示.領(lǐng)航車輛根據(jù)車輛編號與定位距離可計算出該車輛的絕對位置.這種相對定位技術(shù)算法簡單便捷、延時短,定位測距模式采用TOA 方式,測距時間短,精確度高,可有效保證車輛之間的安全距離.

圖5 相對定位測距示意圖Fig.5 Schematic diagram of relative positioning and ranging

領(lǐng)航車輛在尾部安裝雷達(dá),隨行車輛車頭與車尾分別安裝雷達(dá)系統(tǒng).隨行車輛進(jìn)行編號,其向領(lǐng)行車輛發(fā)送信息時攜帶有車輛編號.領(lǐng)航車輛尾部安裝雷達(dá),編號為1 的隨行車輛向領(lǐng)航車輛發(fā)送雷達(dá)信號,領(lǐng)航車輛收到信號后向編號為1 的隨行車輛回送信號,編號為1 的隨行車輛根據(jù)發(fā)送信號與收到信號的時間差計算出兩者之間的距離, 采用這種雙向傳輸測距方式可有效減小單向傳輸信號的不穩(wěn)定性造成的誤差,使測量結(jié)果更為可靠.

當(dāng)編號i+1 隨行車輛的車頭的測距雷達(dá)向編號i 隨行車輛的車尾的測距雷達(dá)發(fā)出電磁波信號時,編號i 隨行車輛收到此信號即向編號i+1 隨行車輛回送響應(yīng)信號,編號i+1 隨行車輛根據(jù)發(fā)射信號與接收到信號的時間差來計算其與編號i 隨行車輛之間的距離,設(shè)光速為c,發(fā)射信號與接收信號的時間差為Δt,則兩車之間的距離為di+1：

4 仿真分析

4.1 卡爾曼（Kalman）濾波算法

Kalman 濾波是一種時域濾波方法,采用狀態(tài)空間方法描述系統(tǒng)[14-15],算法采用遞推形式,數(shù)據(jù)存儲量小,不僅可以處理平穩(wěn)隨機(jī)過程,還可以處理多維和非平穩(wěn)隨機(jī)過程,其作為一種非常重要的最優(yōu)估計理論被廣泛應(yīng)用于導(dǎo)航、定位、目標(biāo)跟蹤、通信與信號處理等領(lǐng)域.

Kalnam 濾波算法中,首先建立兩個狀態(tài)空間模型描述的動態(tài)線性系統(tǒng)方程

式（1）為狀態(tài)方程,式（2）為觀測方程.其中：k 為離散時間,X（k）為系統(tǒng)在k 時刻的狀態(tài),Y（k）為對應(yīng)狀態(tài)的觀測信號;Φ 為狀態(tài)轉(zhuǎn)移矩陣,Γ 為噪聲驅(qū)動矩陣,H 為觀測矩陣;W（k）為輸入的白噪聲,V（k）為觀測噪聲.

在式（3）對應(yīng)的性能指標(biāo)下,根據(jù)遞推射影定理可得

K(k+1)為Kalman 濾波器增益.

ε(k+1)為新息序列,定義為

在假設(shè)1 和假設(shè)2 的條件下,對式（1）兩邊取射影,應(yīng)用射影公式可得

同理對式（2）兩邊取射影,應(yīng)用射影公式可得

新息序列表達(dá)式此時可寫為

Kalman 濾波器和預(yù)報估值誤差為

方差陣為

更新方差陣為

4.2 搭建仿真模型

Kalman 濾波中蘊含遞推估計思想,通過系統(tǒng)輸入與輸出的觀測數(shù)據(jù),對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計[16],利用k 時刻推測k+1 時刻的估計值X（k+1）,并使估計值均方差最小.

車輛行駛過程中獲取的車輛之間相對距離在任何時刻都是有噪聲的,卡爾曼濾波利用目標(biāo)的動態(tài)信息,設(shè)法去掉噪聲的影響,得到一個關(guān)于目標(biāo)位置的好的估計.這個估計可以是對當(dāng)前目標(biāo)位置的估計,也可以是對于將來位置的估計,還可以是對過去位置的估計.卡爾曼濾波就是這樣一個根據(jù)當(dāng)前時刻目標(biāo)的檢測狀態(tài),預(yù)測估計目標(biāo)下一時刻目標(biāo)檢測狀態(tài)的一個動態(tài)迭代循環(huán)過程.本系統(tǒng)采用Kalman濾波對車輛相對距離d 進(jìn)行預(yù)測,校準(zhǔn)測距.假設(shè)測量過程中,真實值會受到加性噪聲V（k）的干擾.再假定任意2 個不同時刻的噪聲互不相關(guān),根據(jù)Kalman 濾波理論,建立系統(tǒng)狀態(tài)估計.根據(jù)推薦性國家標(biāo)準(zhǔn)《智能網(wǎng)聯(lián)汽車自動駕駛功能測試方法及要求第三部分列隊跟馳功能》公開驗證試驗標(biāo)準(zhǔn),車速提升至60 km/h 的編隊行駛要求跟車距離為15 m±20%.仿真過程中,各車輛行駛軌跡為勻速直線運動,設(shè)兩車之間的預(yù)期距離為15 m,初始距離為14 m,初始測量距離為13.7 m.過程噪聲方差Q=0.01,測量引入噪聲方差R=0.04.Φ=1,Γ=1,H=1.則系統(tǒng)狀態(tài)方程和觀測方程分別為

4.3 仿真結(jié)果分析

仿真結(jié)果如圖6 和圖7 所示.由圖6 可以看出,不管是真實值還是觀測值,甚至是Kalman 濾波值,其結(jié)果都小于預(yù)期值,這就表明在系統(tǒng)測量過程中,兩車之間的相對距離預(yù)設(shè)要盡可能的大,以保證兩車之間的安全行駛.由圖7 可以看出,Kalman 濾波與雷達(dá)直接測量的值相比,大大降低了偏差,雖然Kalman 濾波誤差沒有完全消失,但它使?fàn)顟B(tài)盡可能地逼近真實值.另外仿真計算結(jié)果表明,濾波前誤差平均值為0.141 2,經(jīng)過Kalman 濾波后,誤差平均值為0.087 8,誤差減小了37.82%.可見采用卡爾曼濾波后可有效減小誤差,提高定位精度.

圖6 相對距離仿真結(jié)果Fig.6 Relative distance simulation results

圖7 相對距離誤差分析Fig.7 Relative distance error analysis

5 小結(jié)

本文針對車聯(lián)網(wǎng)編隊行駛系統(tǒng)中的車輛定位進(jìn)行了分析,搭建了一種基于相對定位的測距模型,引入了雷達(dá)的TOA 測距方式,在此基礎(chǔ)上采用了Kalman 濾波算法進(jìn)行數(shù)據(jù)降噪處理,從而對編隊中相鄰車輛之間的距離進(jìn)行測算.仿真結(jié)果表明：

（1）基于相對定位方式能夠快速測量相鄰車輛之間的距離,降低了時延,并且避免了絕對坐標(biāo)定位系統(tǒng)覆蓋盲區(qū)的弊端.

（2）基于Kalman 濾波算法能夠有效降低測量過程中誤差的影響,提升了最終的目標(biāo)距離定位結(jié)果,降低了定位誤差.