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

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

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

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

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

1 卡爾曼濾波算法選擇

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

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

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

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

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

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

。

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

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

4 卡爾曼濾波

4.1 卡爾曼濾波遞推過(guò)程

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

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

預(yù)測(cè)階段：

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

（9）

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

（10）

更新階段：

誤差增益矩陣：

（11）

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

（12）

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

（13）

協(xié)方差陣更新：

（14）

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

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

5 實(shí)驗(yàn)分析

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

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

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

5.2 Matlab算法實(shí)現(xiàn)

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

6 結(jié)束語(yǔ)

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

參考文獻(xià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í)時(shí)串口通訊程序設(shè)計(jì)[A].//2007年山東省數(shù)字國(guó)土學(xué)術(shù)交流會(huì)論文集[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]宗長(zhǎng)富，潘釗，胡丹，等.基于擴(kuò)展卡爾曼濾波的信息融合技術(shù)在車(chē)輛狀態(tài)估計(jì)中的應(yīng)用[J].機(jī)械工程學(xué)報(bào)，2009，45（10）：272-277.

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

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

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