基于卡爾曼濾波的汽車航跡推算定位系統(tǒng)研究

揭琳鋒，湯梓恒，周偉偉

（225217 江蘇省 揚州市 揚州工業(yè)職業(yè)技術(shù)學院 交通工程學院）

0 引言

做為智能交通系統(tǒng)（ITS）的重要組成部分之一的車輛定位導航系統(tǒng)，整合了交通地理信息技術(shù)、車輛定位技術(shù)、先進通信技術(shù)等，能有效提高交通工具的機動性、安全性以及道路的通行能力，其發(fā)展水平影響到整個智能交通系統(tǒng)的規(guī)劃發(fā)展。全球衛(wèi)星定位系統(tǒng)（GPS）具有精度高、成本低、定位全球性等特質(zhì)，但是也有著信號易受干擾及被遮蔽的缺陷，而航跡推算（DR）系統(tǒng)具有不易受外界影響、短時間內(nèi)誤差小等優(yōu)點，它們二者相結(jié)合所構(gòu)成的組合導航系統(tǒng)能夠比較全面地發(fā)揮自身優(yōu)勢，并且能夠相互補充，拓展導航系統(tǒng)的使用范圍，使車輛導航系統(tǒng)在諸多惡劣環(huán)境之下都能夠提供穩(wěn)定可靠的導航服務[1-4]。

本文通過融合多級卡爾曼濾波算法，建立了基于卡爾曼濾波的汽車定位系統(tǒng)航跡推算算法，并搭建了全球衛(wèi)星定位系統(tǒng)（GPS）和航跡推算（DR）系統(tǒng)相互配合的組合導航系統(tǒng)，通過MATLAB 軟件進行算法仿真分析。本算法及系統(tǒng)實現(xiàn)了車輛在丟失GPS信號后短時間內(nèi)仍然能夠進行自主定位，并能對行駛軌跡進行分析處理。

1 系統(tǒng)總體方案

GPS/DR 車輛導航系統(tǒng)采用航跡推算（DR）的方法作為全球衛(wèi)星定位系統(tǒng)（GPS）輔助定位方式。當GPS 信號丟失時，系統(tǒng)切換到航跡推算模式，由霍爾里程計實時計算汽車行駛的距離，電子羅盤提供實時航向角，根據(jù)初始時刻的位置經(jīng)緯度信息推算出當前的經(jīng)緯度，達到為車輛定位的目標。

系統(tǒng)采用GPS 和DR 并輔之以卡爾曼濾波法作為車輛定位導航的基本平臺，實現(xiàn)為車輛提供基礎實時導航及短時間自主定位功能。系統(tǒng)主要集成了HOLUX 公司的GR-87 GPS 模塊、斌青公司的BQ-CA81 電子羅盤、Atmel 公司的AT89S52 單片機、LCD1602 顯示器以及它們的外圍濾波、供電電路原件。

在實際算法編制中，需要綜合考慮位置坐標獲取、航向角測量以及行駛里程統(tǒng)計等三方面信息的收集處理。

2 系統(tǒng)算法及誤差分析

航跡推算（DR）定位系統(tǒng)是全球衛(wèi)星定位系統(tǒng)（GPS）輔助系統(tǒng)。當GPS 信號良好，系統(tǒng)算法經(jīng)卡爾曼濾波校正DR 系統(tǒng)的定位信息；當GPS 信號不良，就可以使用DR 數(shù)據(jù)推算車輛位置。實際中兩種系統(tǒng)通過卡爾曼濾波耦合，就可以獲得良好的定位精度和定位可靠性[5]。

航跡推算（DR）算法產(chǎn)生的誤差主要是傳感器誤差引起的位置誤差的積累。一方面是里程計誤差。由于車輪輪胎充氣程度、磨損及載荷不同，打滑等原因造成里程計誤差；另一方面是電子羅盤的誤差。該誤差則由包括傳感器誤差、周圍環(huán)境磁化導致的誤差等引起[6～8]。GPS 還存在定位誤差包括：星歷誤差、GPS 接收機鐘差、觀測誤差等三大類。

2.1 聯(lián)合卡爾曼濾波的選擇與應用

卡爾曼濾波器運用廣泛，它使用狀態(tài)空間概念描述數(shù)學公式，通過遞歸計算求解[9]。在GPS/DR車輛導航系統(tǒng)中，用卡爾曼濾波方法將GPS 和DR的信息同時用于定位解的求解，在濾波過程中修正DR 系統(tǒng)的狀態(tài)。所以，在GPS 失效的情況下，系統(tǒng)自動切換為DR 推算定位。系統(tǒng)算法可以保持較高的定位精度[9]。

文中的GPS/DR 車輛導航系統(tǒng)運用聯(lián)合卡爾曼濾波形式實現(xiàn)多傳感器信息融合。GPS/DR 車輛導航系統(tǒng)通過聯(lián)合卡爾曼濾波技術(shù)，可以很好地實現(xiàn)定位導航信息的最優(yōu)輸出，可以極大地降低傳感器誤差對定位系統(tǒng)狀態(tài)估計的不良影響，提高組合系統(tǒng)的整體濾波精度[10]。系統(tǒng)運用卡爾曼濾波的具體方式如圖1 所示。

圖1 聯(lián)合卡爾曼濾波運行框圖Fig.1 Application of Kalman filter in GPS/DR vehicle navigation system

2.2 算法程序設計

軟件部分主要涉及航跡推算傳感器信號采集、A/D 轉(zhuǎn)換，GPS 信號接收、讀碼，最后將數(shù)據(jù)向系統(tǒng)輸入。

GPS/DR 車輛導航系統(tǒng)開發(fā)基于μVision2IDE集成開發(fā)環(huán)境。該系統(tǒng)采用C 語言編制程序，編輯、編譯均在μVision2 開發(fā)環(huán)境下。開發(fā)完成之后，通過串口助手軟件和單片機程序燒錄板將程序?qū)懭胂到y(tǒng)單片機。

根據(jù)所選擇的硬件和算法的編譯，再通過加入聯(lián)合卡爾曼濾波，便可得到系統(tǒng)軟件設計流程。（1）AT89S52 單片機與GR-87 GPS 模塊的串口通信波特率設置在9 600 bps，當串口通信波特率達到4 800 bps 時系統(tǒng)自動切換而BQ-CA81 電子羅盤數(shù)據(jù)通信。（2）設置串口1 中斷，用來接收GPS 接收模塊送來的信息，在內(nèi)存中開辟一段GPS 信息緩沖區(qū)，每接收一個字節(jié)，寫入緩沖區(qū)，當接收滿一幀的時候，設置輸出標志位，以表示可以輸出GPS信息。檢測一幀是否接收完畢，采用檢測‘’符號的方法，‘’為GPS 輸出每一幀開始的標識[11-15]。

系統(tǒng)軟件流程如圖2 所示。GPS 工作程序流程如圖3 所示。電子羅盤工作程序流程如圖4 所示。

圖2 系統(tǒng)軟件流程圖Fig.2 System software flowchart

圖3 GPS 工作程序流程圖Fig.3 GPS working procedure flow chart

圖4 電子羅盤工作程序流程圖Fig.4 Flow chart of electronic compass working procedure

通過對GPS/DR 車輛導航系統(tǒng)所要達到技術(shù)指標的總體分析，進行硬件和軟件系統(tǒng)的設計與搭建，并進行整合集成，獲得系統(tǒng)總體設計制造方案。提出航跡推算算法，并設計相應軟件流程圖。

3 系統(tǒng)調(diào)試

3.1 硬件器件調(diào)試

設計、組裝GPS/DR 車輛導航系統(tǒng)后，必須經(jīng)過硬件調(diào)試，驗證能否達到功能指標，發(fā)現(xiàn)并改正設計缺陷。主要的調(diào)試方法運用了電路檢查法、電壓、電阻測量法以及信號注入法等。

3.2 仿真測試

本文采用MATLAB 軟件進行仿真實驗。

（1）首先對實驗小車進行運動模型分析，并根據(jù)小車標示規(guī)格參數(shù)估計，對小車運動方程建模，并做簡化：

假定環(huán)境噪聲為信噪比20 dB 高斯白噪聲，得到模擬噪聲信號曲線，如圖5 所示。

圖5 MATLAB 軟件模擬加入高斯白噪聲Fig.5 MATLAB software simulating joining white Gaussian noise

實驗先模擬車輛在無干擾源噪聲情況下的行駛軌跡作為原始參照，如圖6 所示。加入高斯白噪聲干擾，獲得不經(jīng)過卡爾曼濾波的軌跡圖像。然后給程序注入卡爾曼濾波算法，模擬系統(tǒng)在噪聲環(huán)境中融合卡爾曼濾波算法所行駛軌跡，并獲得軌跡圖像。通過仿真試驗驗證航跡推算算法可行性以及融合卡爾曼濾波算法的必要性[16～18]。

圖6 卡爾曼濾波算法程序仿真過程Fig.6 Kalman filter algorithm program simulation process

GPS/DR 車輛導航系統(tǒng)航跡推算（DR）模塊卡爾曼濾波算法MATLAB 仿真結(jié)果如圖7 所示。

通過對仿真圖像的對比分析，可以清楚觀察到在環(huán)境中加入非線性、非平穩(wěn)性高斯白噪聲之后，GPS/DR 車輛導航系統(tǒng)算法程序仿真記錄的位置坐標在原始軌跡周圍震蕩，圖7（b）所示，且幅度較大，會造成系統(tǒng)導航定位的失準。

圖7 卡爾曼濾波算法程序仿真結(jié)果Fig.7 Simulation results of Kalman filter algorithm

通過向系統(tǒng)注入卡爾曼濾波算法之后，由MATLAB 仿真所得到系統(tǒng)位置坐標的波動幅度明顯減小，更加接近路徑的原始軌跡曲線。從而可以判斷，此時由高斯白噪聲對系統(tǒng)信號所造成的影響明顯減弱，系統(tǒng)導航精度偏差降低。試驗證明，融合了卡爾曼濾波算法的GPS/DR 車輛導航系統(tǒng)能夠有效減少噪聲環(huán)境下的導航定位偏差，濾波之后離散的仿真定位數(shù)據(jù)更加接近原始曲線。進而證明卡爾曼濾波算法的加入可以有效提高GPS/DR 車輛導航系統(tǒng)在噪聲環(huán)境中的定位可靠性和實用性。

4 結(jié)論

本文討論了智能交通系統(tǒng)中的核心技術(shù)車輛自主導航技術(shù)的發(fā)展與改進，重點研究了航跡推算算法的發(fā)展與改進，設計了一套基于AT89S52 單片機并結(jié)合全球衛(wèi)星導航系統(tǒng)（GPS）、航跡推算（DR）技術(shù)的組合導航系統(tǒng)架構(gòu)，重點闡述了航跡推算（DR）與卡爾曼濾波融合導航算法，提高了GPS信號遮蔽后系統(tǒng)精度。通過對系統(tǒng)算法程序部分的MATLAB 計算機模擬仿真，航跡推算算法與卡爾曼濾波的融合有效提升了導航系統(tǒng)在GPS 信號遮蔽情況下的導航精度，提高了系統(tǒng)的穩(wěn)定性和實用性。