孫冬凱

（上海同濟(jì)大學(xué)測(cè)繪與地理信息學(xué)院 中國(guó) 上海 200092）

0 前言

移動(dòng)測(cè)量系統(tǒng)是一種多傳感器集成的數(shù)字成圖系統(tǒng)，對(duì)于多傳感器集成空間數(shù)據(jù)采集系統(tǒng)而言最重要的是直接地理坐標(biāo)參考，確定測(cè)量傳感器的坐標(biāo)，使得移動(dòng)測(cè)量系統(tǒng)成為一種獨(dú)立的測(cè)成圖系統(tǒng)[1]。全球定位系統(tǒng)（Global Positioning System,GPS）可以進(jìn)行全球、全天候和實(shí)時(shí)導(dǎo)航，定位精度相關(guān)性小，但是接收機(jī)需要觀察4顆以上衛(wèi)星才能進(jìn)行定位，在高速運(yùn)動(dòng)的情況下，或者城市中受到高大建筑物、隧道等的遮擋時(shí)，會(huì)造成失鎖。航位推算（Dead-Reckoning,DR）通過(guò)一個(gè)方向傳感器和一個(gè)速度傳感器推算車(chē)輛的瞬時(shí)位移增量，在用戶(hù)初始位置已知的情況下求解出位置信息。能夠不依靠外界信息獨(dú)立自主地導(dǎo)航定位，但由于方向傳感器的誤差隨著時(shí)間的延長(zhǎng)而積累，所以DR不能單獨(dú)、長(zhǎng)時(shí)間的提供定位服務(wù)[2]。GPS和DR導(dǎo)航系統(tǒng)各有優(yōu)缺點(diǎn)，通過(guò)卡爾曼濾波的方法建立模型將兩者組合起來(lái)，可以彌補(bǔ)兩者單獨(dú)定位的許多不足。GPS/DR組合系統(tǒng)可以得到平滑、高采樣率的定位結(jié)果，用DR的短時(shí)間定位結(jié)果輔助GPS結(jié)算，避免失鎖等因素對(duì)導(dǎo)航帶來(lái)的影響，而GPS可以輔助DR的定位結(jié)算，避免由于時(shí)間帶來(lái)的DR累積誤差，從而提高導(dǎo)航定位的精度。

1 GPS/DR組合Kalman濾波模型的建立

Kalman濾波方法是一種對(duì)動(dòng)態(tài)系統(tǒng)進(jìn)行實(shí)時(shí)數(shù)據(jù)處理的有效方法，其通過(guò)一系列的觀測(cè)量，采用相應(yīng)的濾波算法估計(jì)出所需的觀測(cè)值。本文采用的GPS/DR組合模型為擴(kuò)展Kalman濾波模型，主要包括狀態(tài)方程和GPS/DR觀測(cè)方程。GPS/DR觀測(cè)方程中的觀測(cè)值包括GPS定位位置解，DR觀測(cè)的角速度與距離。

1.1 GPS/DR組合系統(tǒng)狀態(tài)方程的建立

建立狀態(tài)方程的一個(gè)難點(diǎn)是如何描述車(chē)輛加速度變化。采用機(jī)動(dòng)載體的“當(dāng)前”統(tǒng)計(jì)模型來(lái)描述車(chē)輛加速度的統(tǒng)計(jì)分布。這種模型的意義在于，當(dāng)目標(biāo)現(xiàn)正以某一加速度運(yùn)動(dòng)時(shí)，它在下一瞬時(shí)的加速度取值范圍是有限的，而且只能在“當(dāng)前”加速度的領(lǐng)域內(nèi)[3]。

取組合定位系統(tǒng)的狀態(tài)向量為 X=［xe，ve，ae，xn，vn，an］T，其中 xe、xn分別為車(chē)輛東向和北向的位置分量；ve、vn分別為車(chē)輛東向和北向的速度分量；ae、an分別為車(chē)輛東向和北向的加速度分量。設(shè)采樣周期為T(mén)，得到系統(tǒng)離散的狀態(tài)方程為：

式中 Xk是系統(tǒng)的 n維狀態(tài)向量，Φk，k－1是系統(tǒng)的 n×n維狀態(tài)轉(zhuǎn)移矩陣，Γk，k－1是 n×p 維干擾輸入矩陣，Wk是系統(tǒng)的 p 維隨機(jī)干擾向量。

1.2 GPS/DR組合系統(tǒng)觀測(cè)方程的建立

將GPS輸出的東向位置信息eobs、北向位置信息nobs、DR輸出的角速度w以及里程計(jì)在一個(gè)采樣周期內(nèi)輸出的距離s作為觀測(cè)量，得到系統(tǒng)離散的觀測(cè)方程為

式中，Zk＝［eobs（k） nobs（k） wksk］T

其中v1、v2分別是GPS接收機(jī)輸出的東向位置和北向位置的觀測(cè)噪聲，可近似為的高斯白噪聲；εw為陀螺的漂移，近似為的高斯白噪聲；εs為里程表的觀測(cè)噪聲，近似為（0，σ2s）的高斯白噪聲。

1.3 GPS/DR組合系統(tǒng)Kalman濾波解算

GPS/DR組合系統(tǒng)噪聲滿(mǎn)足Wk和Vk相互獨(dú)立，系統(tǒng)過(guò)程噪聲方差陣非負(fù)定，系統(tǒng)觀測(cè)噪聲方差陣正定，則根據(jù)擴(kuò)展Kalman濾波遞推方程和所建立的GPS/DR組合定位系統(tǒng)的狀態(tài)方程（式（1））和觀測(cè)方程（式（2）），可以得到系統(tǒng)的遞推濾波方程如下[3～6]：

狀態(tài)一步預(yù)測(cè)：一步預(yù)測(cè)誤差方差陣：濾波增益矩陣：

狀態(tài)估計(jì)：

估計(jì)誤差方差陣：

只要給出初值X0和P0，依據(jù)k時(shí)刻的觀測(cè)值Zk，就可以依據(jù)式（3）～式（7）遞推計(jì)算得 k 時(shí)刻的狀態(tài)估計(jì) Xk（k=1,2，…）。

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

圖1 GPS單獨(dú)定位軌跡圖

圖2 GPS/DR組合卡爾曼濾波定位軌跡圖

本次車(chē)載導(dǎo)航試驗(yàn)使用的是同濟(jì)大學(xué)的LD2000型移動(dòng)測(cè)量系統(tǒng)采集的上海高架路實(shí)測(cè)數(shù)據(jù)。由于GPS和DR系統(tǒng)的時(shí)間系統(tǒng)不同，在進(jìn)行組合導(dǎo)航定位時(shí)，需先進(jìn)行時(shí)間同步，將GPS和DR的時(shí)間基準(zhǔn)統(tǒng)一為采用GPS接收機(jī)的時(shí)間。GPS/DR數(shù)據(jù)進(jìn)行組合濾波前，先將GPS接收機(jī)采集的數(shù)據(jù)與GPS基站采集的數(shù)據(jù)進(jìn)行差分，得到GPS輸出的WGS-84大地坐標(biāo)，通過(guò)高斯投影正算轉(zhuǎn)換到當(dāng)?shù)厮阶鴺?biāo)系下，即東向、北向坐標(biāo)。

現(xiàn)已知移動(dòng)測(cè)量系統(tǒng)從坐標(biāo)點(diǎn)（141252.583,3465078.865）出發(fā)，做初速度為零的變加速運(yùn)動(dòng)?？扇V波系統(tǒng)的相關(guān)參數(shù)為：初始狀態(tài)向量X0=[141252.583,0,0,3465078.865,0,0]T，初始預(yù)測(cè)噪聲協(xié)方差陣P0=diag[10,1,0.02,10,1,0.02]，選取的各方差值為GPS/DR組合數(shù)據(jù)經(jīng)過(guò)卡爾曼濾波前后的軌跡對(duì)比如圖1、2所示。

圖1為GPS數(shù)據(jù)得到的定位軌跡，由于橋面遮擋等原因，GPS信號(hào)會(huì)出現(xiàn)短暫的失鎖，導(dǎo)致GPS不能正常工作，造成定位信息的缺失。GPS/DR的組合系統(tǒng)在GPS失鎖時(shí)，可以通過(guò)DR系統(tǒng)進(jìn)行車(chē)輛的航位推算，補(bǔ)充GPS不能正常工作時(shí)的定位信息，并且不會(huì)出現(xiàn)誤差的累積。通過(guò)兩者的信息的融合，穩(wěn)定有效的給出了移動(dòng)測(cè)量系統(tǒng)整個(gè)數(shù)據(jù)采集過(guò)程的運(yùn)動(dòng)軌跡。通過(guò)比較，GPS/DR組合定位有效提高了單一系統(tǒng)動(dòng)態(tài)定位的可靠性。

3 總結(jié)

本文提出了基于GPS/DR觀測(cè)數(shù)據(jù)的組合定位的Kalman濾波模型，并用改模型對(duì)實(shí)測(cè)數(shù)據(jù)進(jìn)行計(jì)算。通過(guò)與原GPS觀測(cè)數(shù)據(jù)的定位結(jié)果對(duì)比表明，應(yīng)用Kalman濾波模型處理GPS/DR數(shù)據(jù)進(jìn)行定位，能夠有效的解決因衛(wèi)星失鎖導(dǎo)致的GPS定位失效以及DR單獨(dú)定位誤差累積的問(wèn)題，提高定位系統(tǒng)的可靠性和準(zhǔn)確性，為移動(dòng)測(cè)量系統(tǒng)提供更好的定位服務(wù)支持。

［1］于興旺，張小紅，劉經(jīng)南，等.GPS/DR原始觀測(cè)值組合導(dǎo)航定位研究[J].武漢大學(xué)學(xué)報(bào)：信息科學(xué)版，2009，02.

［2］楊曉云，何恒，施滸立.車(chē)輛GPS/DR組合導(dǎo)航系統(tǒng)研究[J].測(cè)繪通報(bào)，2007（9）.

［3］付夢(mèng)印，鄧志紅，張繼偉.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京：科學(xué)出版社，2003.

［4］汪志明，花向紅，陳遠(yuǎn)，等.GPS/DR組合Kalman濾波模型及性能分析[J].武漢大學(xué)學(xué)報(bào)：信息科學(xué)版，2009，09.

［5］楊元喜.自適應(yīng)動(dòng)態(tài)導(dǎo)航定位[M].北京：測(cè)繪出版社，2006.

［6］劉友文.基于聯(lián)邦式Kalman濾波的 GPS/DR組合定位技術(shù)[J].機(jī)電技術(shù)，2010（5）.