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

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

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

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

1 編隊(duì)行駛原理

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

圖1 編隊(duì)行駛示意圖Fig.1 Formation driving diagram

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

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

2 編隊(duì)行駛車輛定位方法

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

領(lǐng)航車輛安裝有GPS 和5G 通信模塊,因?yàn)轭I(lǐng)航車輛需要進(jìn)行行駛路徑導(dǎo)航,需要獲取領(lǐng)航車輛的絕對(duì)地理位置坐標(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)航定位的信號(hào),GPS 用戶正是利用這些信號(hào)來(lái)進(jìn)行工作的.GPS的控制部分由分布在全球的由若干個(gè)跟蹤站所組成的監(jiān)控系統(tǒng)所構(gòu)成,根據(jù)其作用的不同,這些跟蹤站又被分為主控站、監(jiān)控站和注入站.GPS 的用戶部分由GPS 接收機(jī)、數(shù)據(jù)處理軟件及相應(yīng)的用戶設(shè)備如計(jì)算機(jī)氣象儀器等所組成.它的作用是接收GPS 衛(wèi)星所發(fā)出的信號(hào),利用這些信號(hào)進(jìn)行導(dǎo)航定位等工作.

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

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

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

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

2.2 隨行車輛定位

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

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

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

3.1 雷達(dá)傳感器

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

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

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

3.2 TOA 測(cè)距方式

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

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

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

3.3 隨行車輛測(cè)距模型

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

圖5 相對(duì)定位測(cè)距示意圖Fig.5 Schematic diagram of relative positioning and ranging

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

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

4 仿真分析

4.1 卡爾曼（Kalman）濾波算法

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

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

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

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

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

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

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

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

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

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

方差陣為

更新方差陣為

4.2 搭建仿真模型

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

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

4.3 仿真結(jié)果分析

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

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

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

5 小結(jié)

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

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

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