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

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

（河南科技學院信息工程學院,河南 新鄉 453003）

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

1 編隊行駛原理

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

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

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

圖2 系統整體原理框圖Fig.2 System overall principle block diagram

2 編隊行駛車輛定位方法

2.1 領航車輛定位

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

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

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

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

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

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

2.2 隨行車輛定位

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

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

3 編隊隨行車輛相對定位實現

3.1 雷達傳感器

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

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

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

3.2 TOA 測距方式

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

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

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

3.3 隨行車輛測距模型

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

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

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

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

4 仿真分析

4.1 卡爾曼（Kalman）濾波算法

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

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

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

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

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

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

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

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

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

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

方差陣為

更新方差陣為

4.2 搭建仿真模型

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

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

4.3 仿真結果分析

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

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

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

5 小結

本文針對車聯網編隊行駛系統中的車輛定位進行了分析,搭建了一種基于相對定位的測距模型,引入了雷達的TOA 測距方式,在此基礎上采用了Kalman 濾波算法進行數據降噪處理,從而對編隊中相鄰車輛之間的距離進行測算.仿真結果表明：

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

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