基于聯合卡爾曼濾波的道路養護施工機械定位精度研究

李曄+++胡權+++王波

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

關鍵詞：道路養護；施工質量監測；GPS；聯合卡爾曼濾波；數據融合

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

1 卡爾曼濾波算法選擇

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

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

聯合卡爾曼濾波器利用信息分配原理可以實現經緯度數據與速度數據的最優融合估計，并使整個系統具有一定的容錯能力，對誤差較大的子系統進行誤差補償，從而能夠獲得整體上最優的性能[6]。文章設計的聯合濾波器的結構見圖1。

2 系統狀態方程的建立

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

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

。

3 系統觀測方程的建立

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

4 卡爾曼濾波

4.1 卡爾曼濾波遞推過程

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

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

預測階段：

狀態一步預測：

（9）

一步預測協方差陣：

（10）

更新階段：

誤差增益矩陣：

（11）

子系統1狀態更新：

（12）

子系統2狀態更新：

（13）

協方差陣更新：

（14）

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

4.2 聯合卡爾曼濾波器

5 實驗分析

5.1 數據采集與預處理

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

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

5.2 Matlab算法實現

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

6 結束語

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

參考文獻

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

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

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

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

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

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

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

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