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

尹立強,張鶴楊,霍俊臣,陳文浩,劉亞飛,朱文成

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

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

1 編隊行駛原理

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

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

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

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

2 編隊行駛車輛定位方法

2.1 領航車輛定位

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

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

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

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

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

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

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

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

3.1 雷達傳感器

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

圖3 雷達工作原理圖Fig.3 Schematic diagram of radar operation

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

3.2 TOA 測距方式

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

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

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

3.3 隨行車輛測距模型

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

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

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

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

4 仿真分析

4.1 卡爾曼（Kalman）濾波算法

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

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

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

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

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

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

在假設1 和假設2 的條件下,對式（1）兩邊取射影,應用射影公式可得

同理對式（2）兩邊取射影,應用射影公式可得

新息序列表達式此時可寫為

Kalman 濾波器和預報估值誤差為

方差陣為

更新方差陣為

4.2 搭建仿真模型

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

車輛行駛過程中獲取的車輛之間相對距離在任何時刻都是有噪聲的,卡爾曼濾波利用目標的動態(tài)信息,設法去掉噪聲的影響,得到一個關(guān)于目標位置的好的估計.這個估計可以是對當前目標位置的估計,也可以是對于將來位置的估計,還可以是對過去位置的估計.卡爾曼濾波就是這樣一個根據(jù)當前時刻目標的檢測狀態(tài),預測估計目標下一時刻目標檢測狀態(tài)的一個動態(tài)迭代循環(huán)過程.本系統(tǒng)采用Kalman濾波對車輛相對距離d 進行預測,校準測距.假設測量過程中,真實值會受到加性噪聲V（k）的干擾.再假定任意2 個不同時刻的噪聲互不相關(guān),根據(jù)Kalman 濾波理論,建立系統(tǒng)狀態(tài)估計.根據(jù)推薦性國家標準《智能網(wǎng)聯(lián)汽車自動駕駛功能測試方法及要求第三部分列隊跟馳功能》公開驗證試驗標準,車速提升至60 km/h 的編隊行駛要求跟車距離為15 m±20%.仿真過程中,各車輛行駛軌跡為勻速直線運動,設兩車之間的預期距離為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é)果都小于預期值,這就表明在系統(tǒng)測量過程中,兩車之間的相對距離預設要盡可能的大,以保證兩車之間的安全行駛.由圖7 可以看出,Kalman 濾波與雷達直接測量的值相比,大大降低了偏差,雖然Kalman 濾波誤差沒有完全消失,但它使狀態(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)中的車輛定位進行了分析,搭建了一種基于相對定位的測距模型,引入了雷達的TOA 測距方式,在此基礎上采用了Kalman 濾波算法進行數(shù)據(jù)降噪處理,從而對編隊中相鄰車輛之間的距離進行測算.仿真結(jié)果表明：

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

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