基于聯(lián)合卡爾曼濾波的道路養(yǎng)護施工機械定位精度研究

李曄+++胡權(quán)+++王波

摘 要：針對道路養(yǎng)護施工質(zhì)量監(jiān)測系統(tǒng)，為提高施工機械的定位精度，設(shè)計了一種基于GPS數(shù)據(jù)的聯(lián)合卡爾曼濾波器。GPS數(shù)據(jù)包含位置和速度信息，運用標準卡爾曼濾擴波修正定位子系統(tǒng)，選用展卡爾曼濾波修正測速子系統(tǒng)，主系統(tǒng)給子系統(tǒng)的運算結(jié)果分配系數(shù)，并進行最優(yōu)估計，從而利用速度信息校正位置，提高定位精度。實驗結(jié)果表明，算法能夠滿足對施工機械準確定位的要求，有助于道路養(yǎng)護管理部門對施工過程進行質(zhì)量控制。

關(guān)鍵詞：道路養(yǎng)護；施工質(zhì)量監(jiān)測；GPS；聯(lián)合卡爾曼濾波；數(shù)據(jù)融合

道路養(yǎng)護施工質(zhì)量監(jiān)測系統(tǒng)通過對道路施工作業(yè)參數(shù)進行采集處理，使決策部門能實時掌握道路施工狀況和施工機械的運行狀態(tài)，有利于對施工過程進行質(zhì)量控制。為了便于管理部門實時掌控施工機械的位置，并計算有效施工距離，系統(tǒng)需要對施工機械進行準確定位。道路養(yǎng)護施工質(zhì)量監(jiān)測系統(tǒng)采用GPS技術(shù)實現(xiàn)施工機械定位。由于受到民用GPS定位精度不高的限制，監(jiān)測系統(tǒng)對施工機械的定位存在較大誤差，不能滿足實時監(jiān)測施工質(zhì)量的需求。GPS的定位誤差來自于接收設(shè)備和衛(wèi)星信號的傳播過程，主要包括衛(wèi)星星歷誤差、衛(wèi)星鐘誤差、接收機鐘誤差以及大氣折射誤差[1]。通過車載終端的GPS模塊可以觀測到施工機械的經(jīng)緯度坐標、速度及方位角信息。根據(jù)NEMA-0183標準協(xié)議，經(jīng)緯度數(shù)據(jù)和速度方位角數(shù)據(jù)分別通過GPRMC、GPVTG兩種標準協(xié)議解析獲得，且GPS獲取經(jīng)緯度和速度的方式不同[2]。因此，文章將GPS定位和GPS測速看作兩個相互獨立的子系統(tǒng)，采用濾波處理、數(shù)據(jù)融合等方法可使位置數(shù)據(jù)與速度數(shù)據(jù)相互校驗，從而提高GPS定位精度，達到對道路養(yǎng)護施工機械進行實時監(jiān)測的目的。

1 卡爾曼濾波算法選擇

濾波理論是在對系統(tǒng)可觀測信號進行測量的基礎(chǔ)上，根據(jù)一定的濾波準則，采用某種統(tǒng)計量最優(yōu)方法，對系統(tǒng)的狀態(tài)進行估計的理論和方法。最優(yōu)濾波要求信號或狀態(tài)的最優(yōu)估值應(yīng)與相應(yīng)的真實值的誤差的方差最小[3]。卡爾曼濾波（Kalman Filter，KF）采用時域狀態(tài)空間方法，把信號過程視為白噪聲作用下的一個線性系統(tǒng)的輸出，用狀態(tài)方程來描述這種輸入-輸出關(guān)系，估計過程中利用系統(tǒng)狀態(tài)方程、觀測方程和白噪聲激勵的統(tǒng)計特性形成濾波算法。由于所用的信息都是時域內(nèi)的量，卡爾曼濾波不但可以對平穩(wěn)的一維隨機過程進行估計，也可以對非平穩(wěn)的、多維隨機過程進行評估，適用于GPS數(shù)據(jù)的處理分析[4]。同時卡爾曼濾波是一種遞推算法，便于在文章的車載終端上實現(xiàn)實時應(yīng)用。對于非線性系統(tǒng)濾波問題，常用的處理方法是利用線性化技巧將其轉(zhuǎn)化為一個近似的線性濾波問題，其中應(yīng)用最廣泛的方法是擴展卡爾曼濾波（Extended Kalman Filter，EKF）。對于非線性系統(tǒng)，首先圍繞濾波值將非線性函數(shù)展開成泰勒級數(shù)，并略去二階及以上項，得到一個近似的線性化模型，然后應(yīng)用卡爾曼濾波完成對目標的濾波估計等處理[5]。

文章采用了聯(lián)合卡爾曼濾波法，將車載終端的GPS系統(tǒng)劃分為定位系統(tǒng)和測速系統(tǒng)兩個相互獨立的子系統(tǒng)。作為線性系統(tǒng)，定位子系統(tǒng)采用標準卡爾曼濾波器（KF），僅將經(jīng)緯度觀測值作為系統(tǒng)輸入；而速度和方位角觀測值則作為測速子系統(tǒng)的輸入，因此測速子系統(tǒng)應(yīng)建立適用于非線性系統(tǒng)的濾波器，文章選用擴展卡爾曼濾波器（EKF）。

聯(lián)合卡爾曼濾波器利用信息分配原理可以實現(xiàn)經(jīng)緯度數(shù)據(jù)與速度數(shù)據(jù)的最優(yōu)融合估計，并使整個系統(tǒng)具有一定的容錯能力，對誤差較大的子系統(tǒng)進行誤差補償，從而能夠獲得整體上最優(yōu)的性能[6]。文章設(shè)計的聯(lián)合濾波器的結(jié)構(gòu)見圖1。

2 系統(tǒng)狀態(tài)方程的建立

在道路養(yǎng)護施工過程中，主要關(guān)注施工機械的經(jīng)緯度位置及行駛速度等信息，無需獲取其高度信息。因此，車載終端只解析與時間、經(jīng)緯度、速度、方位角相關(guān)的GPS協(xié)議報文，并將經(jīng)緯度坐標系轉(zhuǎn)換為xOy投影坐標系，x方向、y方向分別為地理東向和北向，坐標單位為米，以便于建立施工機械位置坐標與速度的關(guān)系模型[7]。

在實際道路養(yǎng)護施工過程中，施工機械的行駛狀態(tài)可以看作勻速直線運動。以車輛的二維平面位置和速度作為狀態(tài)變量，假定過程噪聲和觀測噪聲為高斯白噪聲。將該模型看成是一個線性系統(tǒng)，以便于采用卡爾曼濾波的方法對數(shù)據(jù)進行處理。設(shè)狀態(tài)向量為

。

3 系統(tǒng)觀測方程的建立

對于定位子系統(tǒng)（子系統(tǒng)1），取其系統(tǒng)狀態(tài)向量X1=X，子系統(tǒng)狀態(tài)方程與系統(tǒng)狀態(tài)方程相同。其觀測量為GPS模塊接收的車輛位置坐標xobs和yobs。離散線性子系統(tǒng)的觀測方程為

4 卡爾曼濾波

4.1 卡爾曼濾波遞推過程

在進行卡爾曼濾波之前，需要對濾波初值進行選取。結(jié)合道路養(yǎng)護施工特點，取目標初始坐標和速度作為狀態(tài)向量初值，即

令誤差方差矩陣初值P（0）=10I4；取離散化后的采樣時間T=1s。以此為初始條件進行卡爾曼濾波，濾波過程可以分為以下兩個階段：

預(yù)測階段：

狀態(tài)一步預(yù)測：

（9）

一步預(yù)測協(xié)方差陣：

（10）

更新階段：

誤差增益矩陣：

（11）

子系統(tǒng)1狀態(tài)更新：

（12）

子系統(tǒng)2狀態(tài)更新：

（13）

協(xié)方差陣更新：

（14）

由于兩個子系統(tǒng)的狀態(tài)方程相同、觀測方程不同，在分別進行卡爾曼濾波的過程中，兩個子系統(tǒng)狀態(tài)更新的公式（20）和（21）存在差異，其余步驟公式相同，分別將各自參數(shù)代入計算即可。

4.2 聯(lián)合卡爾曼濾波器

5 實驗分析

5.1 數(shù)據(jù)采集與預(yù)處理

本次實驗由家用轎車模擬壓路機施工時的行駛狀態(tài)，速度6 km/h左右，沿直線行駛，路程115 m。由車載終端在轎車行駛期間完成GPS數(shù)據(jù)的采集與存儲，數(shù)據(jù)包括時間、經(jīng)度、緯度、速度及方位角，采樣周期為1s，而后由計算機通過串口讀取車載終端存儲的數(shù)據(jù)，并進行預(yù)處理。

預(yù)處理將采集的經(jīng)緯度坐標轉(zhuǎn)換為投影坐標，并以m為坐標單位；GPS報文中的速度單位為km/h，需要轉(zhuǎn)換為m/s；方位角需要由角度制轉(zhuǎn)換為弧度制，以便進行反三角函數(shù)運算。總之，作為系統(tǒng)輸入的各項數(shù)據(jù)，單位必須與建立系統(tǒng)觀測方程時的規(guī)定一致。

5.2 Matlab算法實現(xiàn)

圖2中觀測軌跡存在明顯振蕩，說明觀測噪聲的影響非常大。通過對分配系數(shù)進行試取發(fā)現(xiàn)，當(dāng) 時，聯(lián)合卡爾曼濾波器達到最優(yōu)綜合，濾波軌跡最為接近壓路機的真實運動軌跡。聯(lián)合卡爾曼濾波使壓路機定位精度得到明顯提高，有助于進一步增強施工質(zhì)量監(jiān)測效果。

6 結(jié)束語

文章針對道路養(yǎng)護施工質(zhì)量監(jiān)測系統(tǒng)，采用聯(lián)合卡爾曼濾波克服車載GPS模塊精度不足的缺點，從而提高施工機械定位精度。對于定位子系統(tǒng)采用標準卡爾曼濾波，對于測速非線性子系統(tǒng)則采用擴展卡爾曼濾波，主濾波器采用分配系數(shù)對兩個子系統(tǒng)的輸出結(jié)果進行最優(yōu)估計。實驗結(jié)果表明，算法能夠滿足對施工機械準確定位的要求，從而易于描繪出施工機械的運行軌跡，有助于道路養(yǎng)護管理部門對施工過程進行質(zhì)量監(jiān)測。

參考文獻

[1]Choi E， Cicci D A. Analysis of GPS static positioningproblems[J]. Applied Mathematics and Computation，2003，140（1）：37-51.

[2]韓友美，鐘政，桑逢云，等.NEMA-0183協(xié)議下GPS數(shù)據(jù)的實時串口通訊程序設(shè)計[A].//2007年山東省數(shù)字國土學(xué)術(shù)交流會論文集[C].2007：198-201.

[3]Simon Haykin.自適應(yīng)濾波器原理（第四版）[M].北京：電子工業(yè)出版社，2003：1-4.

[4]杜曉輝，任章.基于卡爾曼濾波的GPS靜態(tài)定位精度分析[J].全球定位系統(tǒng)，2008，33（5）：47-51.

[5]宗長富，潘釗，胡丹，等.基于擴展卡爾曼濾波的信息融合技術(shù)在車輛狀態(tài)估計中的應(yīng)用[J].機械工程學(xué)報，2009，45（10）：272-277.

[6]叢玉良，陳萬忠，孫永強，等.基于聯(lián)合卡爾曼濾波器的交通信息融合算法研究[J].公路交通科技，2010，27（7）：114-117.

[7]徐紹銓，張華海，楊志強，等.GPS測量原理及應(yīng)用（第三版）[M]. 武漢：武漢大學(xué)出版社，2011：1-10.

[8]黃小平，王巖.卡爾曼濾波原理及應(yīng)用：MATLAB仿真[M].北京：電子工業(yè)出版社，2015：77-79.