車載GPS_DR組合導(dǎo)航系統(tǒng)數(shù)據(jù)融合算法研究

1、車載 GPS/ D R 組合導(dǎo)航系統(tǒng)數(shù)據(jù)融合算法研究常青 ,鄭平方 ,柳重堪 ,張其善( 北京航空航天大學(xué)電子工程系 ,北京 100083)摘 要 :本文介紹了車載 GPS/ DR 組合導(dǎo)航系統(tǒng)的設(shè)計(jì) ,建立了表示車輛加速度及影響組合導(dǎo)航系統(tǒng)定位精度的主要誤差的數(shù)學(xué)模型 。針對(duì)迭代擴(kuò)展組合卡爾曼濾波算法中 ,由于 DR 系統(tǒng)誤差的 引入導(dǎo)致濾波效果不好的問題 ,提出了一種新的組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合算法 。對(duì)實(shí)際跑車數(shù)據(jù)的處理結(jié)果表明 ,該算法在提高組合導(dǎo)航系統(tǒng)的定位精度及可靠性和完整性方面是有效的 。關(guān)鍵詞 :全球定位系統(tǒng) ;組合導(dǎo)航 ;航位推算法 ;數(shù)據(jù)融合中圖分類號(hào) :V47412文章編號(hào)

2、 :10002436X(2000) 0220042207文獻(xiàn)標(biāo)識(shí)碼 :AThe Studyof Data Fusion Algorithm of GPS/ D RIntegrated Vehicular Navigation SystemCHANG Qing ,ZHENG Ping2fang ,L IU Zhong2kan ,ZHANG Qishan(Department of Electronic Engineering ,Beijing University of Aeronautics and Astronautics ,Beijing 100083 ,China)Abstract :

3、In this paper ,the design of the GPS/ DR integrated vehicular navigation system is presented ,and themathematical models are founded for the acceleration of vehicle and the main errors which affect the positioning accuracy of the integrated vehicular navigation system1Also ,a new data fusion algorit

4、hm of the integrated vehicu2 lar navigation system is proposed to solve the problem of unsatisfactory filtering effect due to the introduction of the DR system error in the iterated , extended and federated Kalman filter algorithm1The experimental results show that this algorithm is effective for th

5、e improvement of the positioning accuracy , reliability and integrity of the integrated vehicular navigation system1Key words :global positioning system ;integrated navigation ; dead reckoning ; data fusion1引言全球定位系統(tǒng) ( GPS) 是美國國防部研制的第二代衛(wèi)星導(dǎo)航定位系統(tǒng)1 ,它為全球用戶提 供全天候 、連續(xù) 、實(shí)時(shí)的高精度位置 、速度和時(shí)間信息 ,因此在軍 、民方面都具有重要的開發(fā)

6、和 利用價(jià)值 。目前 , GPS 定位技術(shù)除了廣泛應(yīng)用于飛機(jī)和水面船只的導(dǎo)航定位外 ,在陸地車輛的 導(dǎo)航定位中也獲得了廣泛的應(yīng)用 。車輛導(dǎo)航定位系統(tǒng)是在進(jìn)入九十年代以后發(fā)展起來的一項(xiàng) 新技術(shù) ,近年來 ,隨著城市車輛數(shù)量的日益增多 ,如何在城市里有效地指揮和管理公共服務(wù)車 輛及監(jiān)控一些特種專用車輛已成為交通運(yùn)輸部門 ,城市規(guī)劃部門及金融 、公安 、保密等特殊部 門面臨的一個(gè)重要問題 ,這使得車輛導(dǎo)航定位系統(tǒng)更加受到關(guān)注2 4 。航位推算5 (DR2Dead收稿日期 :1999203216 ;修訂日期 :1999211211基金項(xiàng)目 :國家重點(diǎn)科技攻關(guān)計(jì)劃項(xiàng)目 (9621302205202)學(xué)術(shù)

7、論文Reckoning) 和 GPS 是兩種不同的車輛定位技術(shù) ,二者各有弱點(diǎn) 。DR 系統(tǒng)的弱點(diǎn)是定位誤差隨時(shí)間的推移發(fā)散 ,且需要由其它系統(tǒng)提供車輛的初始位置和初始方位角 ,因此 DR 系統(tǒng)不能單 獨(dú)長時(shí)間使用 ,目前的車輛導(dǎo)航定位系統(tǒng)均以 GPS 作為定位手段 。GPS 系統(tǒng)的弱點(diǎn)是當(dāng)車輛穿行在城市高樓區(qū) 、林蔭道 、涵洞等處時(shí) ,常出現(xiàn)衛(wèi)星信號(hào)遮擋問題 ,導(dǎo)致 GPS 的功能失效 ,不 能正常定位 ,這一弱點(diǎn)是由 GPS 系統(tǒng)采用的被動(dòng)定位方式?jīng)Q定的 。如果將兩個(gè)系統(tǒng)組合起 來 ,就可以在出現(xiàn)衛(wèi)星信號(hào)遮擋問題時(shí) ,利用 DR 系統(tǒng)的完全自主性對(duì) GPS 系統(tǒng)進(jìn)行補(bǔ)充 ,因此組合系統(tǒng)比某

8、一個(gè)單獨(dú)的系統(tǒng)具有更好的可靠性和完整性 ,于是 GPS/ DR 組合導(dǎo)航系統(tǒng)應(yīng) 運(yùn)而生 。數(shù)據(jù)融合算法是組合導(dǎo)航系統(tǒng)研究中的一個(gè)關(guān)鍵問題 ,這個(gè)問題解決的好壞 ,決定了 能否進(jìn)一步提高系統(tǒng)的定位精度及可靠性和完整性 。本文首先介紹了車載 GPS/ DR 組合導(dǎo)航系統(tǒng)的設(shè)計(jì) ,建立了該導(dǎo)航系統(tǒng)的數(shù)學(xué)模型 ,然后針對(duì)迭代擴(kuò)展組合卡爾曼濾波算法中 ,由于 DR 系統(tǒng)誤差的引入導(dǎo)致濾波效果不好的問題 ,提出了一種新的組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合算 法 ,對(duì)實(shí)際跑車數(shù)據(jù)的處理結(jié)果證明了算法的有效性 。2車載 GPS/ D R 組合導(dǎo)航系統(tǒng)的組成車載 GPS/ DR 組合導(dǎo)航系統(tǒng)的組成如圖 1 所示 ,其中 ,

9、中心控制器完成對(duì) GPS 、陀螺和里 程儀的數(shù)據(jù)采集 ,然后進(jìn)行數(shù)據(jù)融合 ,輸出定位結(jié)果 。數(shù)據(jù)融合算法對(duì)整個(gè)系統(tǒng)的定位精度起 關(guān)鍵作用 。圖 1 車載 GPS/ DR 組合導(dǎo)航系統(tǒng)的組成3 基于迭代擴(kuò)展卡爾曼濾波方法的數(shù)據(jù)融合算法311離散狀態(tài)方程的建立與航空導(dǎo)航不同的是陸地車輛導(dǎo)航是在二維平面內(nèi)進(jìn)行的 ,因此組合導(dǎo)航系統(tǒng)的卡爾曼 濾波狀態(tài)變量取為X = X TX T T12X T = e n eX T = ;其中nen ;12e nse 和 n 分別為東向位置和北向位置 , 單位為m ; e 和 n 分別為東向速度和北向速度 , 單位為m/ s ;e 和 n 分別為東向加速度和北向加速度

10、, 單位為m/ s2 ;e 和n 分別為各種誤差源在兩個(gè)坐標(biāo)軸 上造成的總的位置誤差 , 單位為 m ;為陀螺輸出的相對(duì)轉(zhuǎn)角誤差 , 單位為 rad ;s 為里程儀輸 出的距離誤差 , 單位為 m 。由文獻(xiàn) 4 知 ,可采用一階馬爾可夫過程描述 ,s ,e和n , 即 = - + s = - S + e = - e + n = - n + sseenn其中 , , , 分別為對(duì)應(yīng)馬爾可夫過程的相關(guān)時(shí)間常數(shù)的倒數(shù) ; , , , 是均值senSen為零 、方差分別為 q2 , q2 , q2 , q2 的高斯白噪聲 。由于受到許多外界因素的影響 , 如路線形Sen據(jù)文獻(xiàn) 6 的計(jì)算結(jié)果 , 這一

11、過程是連續(xù) 、有界 、零均值的平穩(wěn)隨機(jī)過程 。考慮到城市環(huán)境下車輛行駛過程的復(fù)雜性和 Singer 模型的局限性7 , 應(yīng)采用機(jī)動(dòng)加速度的“當(dāng)前”統(tǒng)計(jì)模型 7 描述 車輛加速度 。所謂機(jī)動(dòng)加速度的“當(dāng)前”統(tǒng)計(jì)模型 , 即令e = ae + aeae = - a ae + an = an + an a n = - a an + aeenn其中 ae , an為零均值有色加速度噪聲 ; ae , an為機(jī)動(dòng)加速度均值 , 它在每一采樣周期內(nèi)為常數(shù) ;a ,a 為對(duì)應(yīng)馬爾可夫過程的相關(guān)時(shí)間常數(shù)的倒數(shù) ; a , a 是均值為零 、方差分別為 2a ,enene2, 22 的高斯白噪聲 。根據(jù)以上模型

12、和典型的離散處理方法 , 并假設(shè)數(shù)據(jù)采樣周期為 T ,a a ae n n得到系統(tǒng)的離散狀態(tài)方程為X ( k + 1) = ( k + 1) X ( k) + U ( k) + W ( k)(1)108 202 8X ( k) = X T ( k)X T ( k) T為 a b 階零矩陣 , 下同)其中 =(012a b2X Te ( k)n ( k)1 ( k) = e ( k)n ( k)e ( k)n ( k) e ( k) n ( k) X T2 ( k) = ( k)S ( k) I2 202 202 202 2TI2 2I2 202 202 2C1C2E102 202 202 20

13、2 2e - T0e - T1 =( I2 2為二階單位矩陣 , 下同)2 =0SE2ci ( i = 1 , 2 , 3 , 4) 7 C1 = diag c1 , c2C2 = diag c3 , c4E1 = diag e - ae T , e - an T E2 = daig e - e T , e - n T01 4 Tui ( i = 1 , 2 , 3 , 4 , 5 , 6) 7 U ( k) = u1u2u3u4u5u6W ( k) 為離散時(shí)間白噪聲序列 。系統(tǒng)過程噪聲協(xié)方差矩陣為08 2Q102 8Q ( k) = E W ( k) W T ( k) =Q2Q11 06 2

14、Q2 = diag2 ( 1 -e - 2 ) ,T 2- 2 T其中Q1 =( 1 - eS ) (1 -S02 6Q12Q11見文獻(xiàn) 7 , Q12 = diag2 (1 -e - 2e T) ,2e - 2n T) 。en312觀測方程的建立將 GPS 接收機(jī)輸出的位置 er , nr 、速度 e r , n r及陀螺和里程儀輸出的距離和相對(duì)轉(zhuǎn)角 S 和作為外部觀測量 , 則外部觀測量與狀態(tài)量之間具有如下關(guān)系z1 ( k) = er ( k) = e ( k) +e ( k) + v1 ( k)z2 ( k) = nr ( k) = n ( k) +n ( k) + v2 ( k)z3

15、( k) = e r ( k) = e ( k) + v3 ( k)z4 ( k) = n r ( k) = n ( k) + v4 ( k)z5 ( k) =( k) = ( k) - ( k - 1) + ( k) + v5 ( k)z6 = S ( k) = Te2 ( k) + n2 ( k) + S ( k) + v6 ( k)其中 ( k) 為 k T 瞬時(shí)車輛運(yùn)動(dòng)方向與北向的夾角 , 以順時(shí)針為正 。( k) 和 ( k - 1) 為k - 1( k) = arctan e ( k)( k - 1) = (0) + ( i ) + ( i ) + n ( k)i = 1取 0 ,

16、 - 三個(gè)值中之一 ; vi ( k ) ( i = 1 , 2 , 3 , 4 , 5 , 6) 是均值為零 、方差為 r2 的高斯白噪i聲 。于是觀測方程為Z ( k) = h X ( k) + V ( k)Z ( k) = ZT ( k)ZT ( k) T12ZT ( k) = e ( k)ZT ( k) = ( k)n ( k)e ( k)n ( k) S ( k) 1rrrr2h1 X ( k) h2 X ( k) h X ( k) =10000100001000010000000010000100h1 X ( k) = H1 ( k) X1 ( k) =X1 ( k)arctan

17、e ( k)+ - ( k - 1) + ( k)n ( k)h2 X ( k) =Te2 ( k) + n2 ( k) + S ( k)V ( k) 的均值為零 , 方差為 R ( k) = diag R1 , R2 , R1 = diag r2 , r2 , r2 , r2 , R = diag r2 , r2 。1 2 3 4 25 6由于觀測方程為非線性方程 , 為將其線性化 , 采用擴(kuò)展卡爾曼濾波技術(shù) , 即在一步預(yù)測值處將其展成泰勒級(jí)數(shù) , 并忽略高次項(xiàng) , 得Z ( k) h X ( k , k - 1) + H X ( k , k - 1)X ( k) - X ( k , k

18、- 1) + V ( k)02 2I2 2H 2 202 202 202 202 202 2I2 2I2 202 202 2I2 202 202 2h1h3h2h4其中H X ( k , k - 1) =H 2 2 = n - e h1 = e2 + n2h2 = e2 + n2X ( k , k - 1)X ( k , k - 1) Te Tn h4 =h3 =e2 + n2e2 + n2X ( k , k - 1)X ( k , k - 1)令 ( k) = Z ( k) - h X ( k , k - 1) + H X ( k , k - 1) X ( k , k - 1) ( k) H

19、 X ( k , k - 1) X ( k) + V ( k)(2)式 ( 2) 就是線性化的觀測方程 。觀測方程的線性化必然引入線性化誤差 , 為消除此誤差 , 可做迭代 , 即在得到的濾波估計(jì)處重新將觀測方程泰勒展開 , 這種既使用擴(kuò)展卡爾曼濾波技術(shù) , 又 做迭代的方法稱為迭代擴(kuò)展卡爾曼濾波方法 。313 數(shù)據(jù)融合算法在式 ( 1) 和式 ( 2) 的基礎(chǔ)上 , 利用文獻(xiàn) 7 給出的加速度均值自適應(yīng)的卡爾曼濾波算法 , 可 以得到組合導(dǎo)航系統(tǒng)的迭代擴(kuò)展組合卡爾曼濾波算法 。對(duì)實(shí)際跑車數(shù)據(jù)的處理結(jié)果表明 , 該 算法的效果不好 。主要原因在于陀螺和里程儀的誤差被引入了濾波器的位置 、速度和

20、加速度 的估計(jì)中 。為了提高整個(gè)組合系統(tǒng)的估計(jì)精度 , 首先必須提高位置 、速度和加速度的估計(jì)精度 , 為此應(yīng)將兩個(gè)系統(tǒng)分離 。然后利用這些具有較高精度的估計(jì) , 再對(duì)陀螺和里程儀的誤差進(jìn) 行估計(jì) , 此時(shí)應(yīng)將兩個(gè)系統(tǒng)結(jié)合 , 見圖 2 。只有這樣 , 才能在數(shù)據(jù)融合過程中做到既提高組合 導(dǎo)航系統(tǒng)的定位精度 , 又能將陀螺和里程儀的誤差盡可能準(zhǔn)確地估計(jì)出來 。為此我們提出如下的遞推算法 ( 為消除觀測方程的線性化誤差 , 可做迭代) :1) X ( k , k - 1) = ( k , k - 1) X ( k - 1)2 - 1 T2 I2 2TI2 2I2 202 202 202 202

21、202 2E202 202 202 202 202 2I2 202 202 202 202 2TI2 2I2 202 202 202 2 ( k , k - 1) =其中22) P1 ( k , k - 1) = 1 ( k , k - 1) P1 ( k - 1) T ( k , k - 1) + Q ( k - 1)11P ( k , k - 1) = ( k , k - 1) P ( k - 1) T ( k , k - 1) + Q ( k - 1)3) K1 ( k) = P1 ( k , k - 1) HT ( k) H ( k) P ( k , k - 1) HT ( k) +

22、R ( k) - 111111K( k) = P ( k , k - 1) HT X ( k , k - 1) H X ( k , k - 1) P ( k , k - 1) HT X ( k , k - 1) + R ( k) - 14) X ( k) = X ( k , k - 1) + GTGT T12g2 T其中G1 = K1 ( k) Z1 ( k) - h1 X ( k , k - 1) G2 = g1g1 , g2是列向量 K( k) Z ( k) - h X ( k , k - 1) 的第九和第十個(gè)元素 。5) P1 ( k) = I - K1 ( k) H1 ( k) P1

23、( k , k - 1)P ( k) = I - K( k) H X ( k , k - 1) P ( k , k - 1)圖 2 數(shù)據(jù)融合示意圖假設(shè) k T 時(shí)刻 GPS 接收機(jī)發(fā)生信號(hào)丟失 ,不能給出定位解 ,那么數(shù)據(jù)融合停止 ,此時(shí)由航 位推算系統(tǒng)按下式繼續(xù)給出車輛的位置e ( k ) = e ( k - 1) + s ( k ) sin( k - 1) +( k ) n ( k ) = n ( k - 1) + s ( k ) cos( k - 1) +( k ) ( 3)(4)4實(shí)驗(yàn)驗(yàn)證為了驗(yàn)證數(shù)據(jù)融合算法的有效性 , 1998 年 4 月 3 日進(jìn)行了組合導(dǎo)航系統(tǒng)的跑車實(shí)驗(yàn) , 路

24、線 為北京航空航天大學(xué)校內(nèi)逸夫館 體育館 3 號(hào)樓 學(xué)校正東門 1 號(hào)樓 主樓廣場 , 數(shù)據(jù)采 樣周期為 1 秒 。將 GPS 與航位推算系統(tǒng)按照上面的算法進(jìn)行數(shù)據(jù)融合 ,結(jié)果示于圖 3圖 6 , 計(jì)算時(shí)初始條件選取為 : = = 0101 ,a =a = 1 , = 2000 , = 2000 ,a =a= 018 , = 2e ne nsens = 011 , = = 10 , X (0) = z1 (0) , z2 (0) , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 TenR = diag 102 , 102 , 12 , 12 , 01052 , 22 , P (0

25、) = diag 202 , 202 , 12 , 12 , 0122 , 0122 , 52 , 52 , 0112 , 22 P1 (0) = diag 202 , 202 , 12 , 12 , 0122 , 0122 , 52 , 52由圖 3 知 , 濾波曲線與 GPS 觀測曲線基本一致 。圖 4 所示為圖 3 中 AB 段的局部放大 ,從圖 3 濾波曲線與 GPS 觀測曲線比較圖 4 圖 3 中 AB 段的局部放大圖 4 可以看出 ,濾波曲線對(duì)比于原始曲線有很好的平滑效果 ,達(dá)到了濾波的目的 。圖 5 是用濾波器給出的陀螺和里程儀的誤差估計(jì)對(duì) DR 系統(tǒng)進(jìn)行誤差補(bǔ)償前后的曲線 ,從

26、圖 5 可以看出 , DR 系統(tǒng)經(jīng)過誤差補(bǔ)償后 ,誤差發(fā)散得到有效控制 ,這說明濾波器給出的陀螺和里程儀的誤差估計(jì)是很準(zhǔn)確的 。圖 6 是誤差補(bǔ)償前后的 DR 系統(tǒng)和原始 GPS 系統(tǒng)北向位置差曲線 ( 東向位置差曲線與圖 6 類似) ,從圖 6 可以看出 ,誤差補(bǔ)償后的 DR 系統(tǒng)與 GPS 的北向位置差始終保 持在較小的范圍內(nèi) ,充分證明了算法的有效性 。圖 5 誤差補(bǔ)償前后的 DR 曲線比較圖 6 誤差補(bǔ)償前后的 DR 和 GPS 北向位置差曲線5 結(jié)束語本文提出的算法在給出車輛的位置估計(jì)及陀螺和里程儀的誤差估計(jì)的同時(shí) ,還可以自動(dòng)給出車輛的方位角 (見式 (3) 和式 (4) ) ,陀螺和里程儀的誤差估計(jì)越準(zhǔn)確 ,則給出的方位角越準(zhǔn) 確 。由于當(dāng) GPS 系統(tǒng)丟失信號(hào)時(shí) ,DR 系統(tǒng)是以濾波器給出的位置估計(jì)和方位角作為車輛的 初始位置和初始方位角的 ,因此濾波估計(jì)越準(zhǔn)確 ,切換到 DR 系統(tǒng)后 ,DR 系統(tǒng)正常工作的時(shí)間位置估計(jì)及陀螺和里程儀的誤差估計(jì) ,且計(jì)算結(jié)果還表明 ,當(dāng) GPS 系統(tǒng)丟失信號(hào)時(shí) ,DR 系統(tǒng)在短時(shí)間內(nèi) (至少 150s) 完全可以滿足定位精度的要求 ,這些充分說明了該算法在提高組合導(dǎo) 航系統(tǒng)的定位精度和可靠性 、完整性方面是有效的 ,它將成為組合導(dǎo)航系統(tǒng)的核心 。參考文