位置-姿態(tài)并行濾波的DR／GPS組合導(dǎo)航算法

1、位置-姿態(tài)并行濾波的DRGPS組合導(dǎo)航算法位置一姿態(tài)并行濾波的DR/GPS組合導(dǎo)航算法臧榮春崔平遠(yuǎn)(哈爾濱工業(yè)大學(xué)深空探測基礎(chǔ)研究中心,哈爾濱150001)E-mail: 摘要文章提出了帶有反饋枝正的位置,姿態(tài)并行濾波器結(jié)構(gòu),以滿足各子系統(tǒng)對濾波周期的不同要求.結(jié)合姿態(tài)角子系統(tǒng)特點(diǎn)設(shè)計(jì)了量測噪聲方差陣自動加權(quán)的自適應(yīng)擴(kuò)展卡爾曼濾波算法.仿真結(jié)果表明該算法能夠有效地利用各傳感器信息.給出較好的融合結(jié)果.關(guān)鍵詞航住推算GPS組合導(dǎo)航系統(tǒng)擴(kuò)展卡爾曼濾波文章編號10028331一(2006)07022103文獻(xiàn)標(biāo)識碼A中圈分類號TP391Position-Attitud

2、eParallelFilterforDR,GPSIntegratedNavigationSystemZangRongchunCuiPingyuan(DeepSpaceExplorationResearchCenter,HarbinInstituteofTechnology,Harbin150001)Abstract:Aposition-attitudeparallelfilterisestablishedtosatisfytherequestfordifferentfiltercyclesofsubsystems.AnEKFalgorithmismadeforattitudeKalmanfil

3、ter,whichmeasurementnoiseovariancematrixcanbeweightedadaptively.Simulationresultsindicatea1lsensors'informationcanbeusedeffectively.andglwperfectresult.Keywords:deadreckoning,GPS,integratednavigationsystem,EKFGPS/DR組合導(dǎo)航系統(tǒng)是解決移動車輛在GPs衛(wèi)星信號受遮擋時,仍能有效地定位問題的主要手段之一.早期的GPS/DR組合導(dǎo)航算法沒有考慮航向角的估計(jì)問題1_,只是綜合GPS和

4、DR的導(dǎo)航參數(shù)信息得出定位結(jié)果,一旦GPS失效航位推算系統(tǒng)的航向角偏差將導(dǎo)致位置推算精度較差.隨著算法的深入研究將航向角或航向角誤差作為狀態(tài)量參與濾波估計(jì),但是其狀態(tài)方程是非線性的.線性化過程會引入模型誤差】.而且以往的航位推算往往因?yàn)槭嵌S定位而忽略了俯仰角的計(jì)算.雖然較小的坡度對定位精度的影響遠(yuǎn)小于陀螺等慣性器件的漂移對定位造成的影響.但車輛在大坡度或城市高架橋等地況行駛時車體的俯仰對航向角的測量影響不可忽視.本文就如何利用低成本的DR和GPs進(jìn)行姿態(tài),位置組合進(jìn)行了研究,以卡爾曼濾波為基礎(chǔ),提出了帶反饋的位置,姿態(tài)角并行濾波器結(jié)構(gòu).其狀態(tài)方程都是線性的.既簡化了濾波方程,叉滿足了各子系統(tǒng)

5、對濾波周期的要求.并針對各子系統(tǒng)的特點(diǎn)分別設(shè)計(jì)濾波算法.根據(jù)姿態(tài)角濾波器觀測方程的非線性和觀測量的冗余性.設(shè)計(jì)了噪聲方差陣自動加權(quán)擴(kuò)展卡爾曼濾波算法.文章最后給出了靜態(tài)和動態(tài)試驗(yàn)結(jié)果.驗(yàn)證了該方法的有效性.l位置一姿態(tài)并行濾波器結(jié)構(gòu)多傳感器組合導(dǎo)航系統(tǒng)的主系統(tǒng)一般都是自主式導(dǎo)航,雖然有誤差積累,但不受外界環(huán)境的干擾,可以連續(xù)定位.而衛(wèi)星導(dǎo)航系統(tǒng)定位精度高,無誤差積累,但易受外界環(huán)境干擾,信號一旦丟失將無法定位.因此選其為輔助導(dǎo)航系統(tǒng).本文選取DR為主系統(tǒng).以GPS作為輔助導(dǎo)航系統(tǒng)采用直接濾波法,即以主導(dǎo)航系統(tǒng)輸出的導(dǎo)航參數(shù)(例如東向位置,北向位置等)為濾波器狀態(tài)變量,當(dāng)GPs接收機(jī)能夠正常工作

6、時.輔助導(dǎo)航系統(tǒng)輸出的導(dǎo)航參數(shù)作為觀測量.濾波器輸出的導(dǎo)航參數(shù)估計(jì)值就是系統(tǒng)最終的定位結(jié)果:當(dāng)GPS接收機(jī)受到屏蔽不能正常工作時.主導(dǎo)航系統(tǒng)仍然能通過推算提供導(dǎo)航信息.本文濾波器采用位置,角度分別濾波的并聯(lián)濾波器結(jié)構(gòu).一般來說1Hz的采樣頻率可以滿足位置對數(shù)據(jù)更新率的要求,但遠(yuǎn)不能滿足角度測量要求,本文角速率的采樣頻率為10Hz,同時為避免量測相關(guān)問題要求位置濾波周期要大于GPS的位置時間相關(guān)常數(shù),因此采用位置,角速度并行濾波方式,位置子濾波器以較長的濾波周期處理位置數(shù)據(jù),角度子濾波器以較短的周期處理姿態(tài)角數(shù)據(jù),既滿足了位置和角度對濾波周期要求.叉便于根據(jù)各子系統(tǒng)的特點(diǎn)設(shè)計(jì)相應(yīng)的濾波算法.圖l

7、組合導(dǎo)航系統(tǒng)濾波器結(jié)構(gòu)框圖另外.由于本文采用直接濾波法,其狀態(tài)變量是導(dǎo)航參數(shù)量而非誤差量.分別進(jìn)行位置與角速率濾波可以避免計(jì)算過程中因各變量之間的數(shù)量級差別較大而產(chǎn)生奇異值.2以DR為主系統(tǒng)的動態(tài)模型利用航位推算定位首先要考慮車體姿態(tài)的問題.建立載體坐標(biāo)系b,軸是沿車體橫軸指右方向,y軸是沿車體縱軸指前進(jìn)方向,Z軸與,y軸垂直并構(gòu)成右手坐標(biāo)系.,盧,分別代表載體的三個姿態(tài)角航向角,俯仰角和滾轉(zhuǎn)角,如把載體坐標(biāo)系作為動坐標(biāo)系,地理坐標(biāo)系(即東,北,天坐標(biāo)系)作為參考坐標(biāo)系,則姿態(tài)角,盧,7即為一組歐拉角,其含義如圖2所示:計(jì)算機(jī)工程與應(yīng)用20o6.07221y進(jìn)方向圍2歐拉角與坐標(biāo)軸示意圍用1T

8、表示載體坐標(biāo)系相對地理坐標(biāo)系的角速度在載體坐標(biāo)系軸向的分量構(gòu)成的列矢量.則與姿態(tài)角速率的關(guān)系可以盧y表示如下:朝T_(1)=一蘭0_costcococost0sinTsinT1-cosyDR導(dǎo)航系統(tǒng)由一個雙軸加速度計(jì),兩個單軸陀螺和一個里程計(jì)組成.沿z軸和軸方向安置陀螺,沿z軸和y軸方向安置加速度計(jì).陀螺測量繞z軸和X軸旋轉(zhuǎn)的角速率和峨,加速度計(jì)測量載體前進(jìn)方向的加速度和垂直車體方向的加速度.=是車輛行駛方向與正北方向夾角,順時針為正.正常情況下車輛是平穩(wěn)行駛的,因此認(rèn)為滾轉(zhuǎn)角y0,則姿態(tài)角微分方程:Or-co呂8(2)車輛正常行駛情況下一85.<8<85.,因此以

9、上微分方程在測量范圍內(nèi)是非奇異的考慮車輛滾轉(zhuǎn)角恒為零情況下的DR系統(tǒng)的運(yùn)動學(xué)差分方程:x(k+1)(k)-Iv(J)(3)y(k+1)=y()卻()(4)(J+1)(k)cosj3sinct(5)(J+1)(k)coscos(6)+l=+(7)一(8)cos/Jk其中,y為東向和北向位置,分別為東向和北向速度,為采樣時間間隔,口為車輛前向速度.3位置,姿態(tài)角濾波方程的建立3.1位置子濾波器濾波方程根據(jù)運(yùn)動學(xué)方程,可以得到位置濾波器濾波方稃,選取狀態(tài)變量:1=Ix,.狀態(tài)方程:(k+1)=ll(k)+litl(k)+'I,.(k)(9)l=10010000000000ll1(k)=觀測量

10、:Zl=xcyc,esI)eGp5Z1()=日1Xl+v1()日1=10010O0000O0100l00Av(k)cosinAv(k)cosflcosa2222006.07計(jì)算機(jī)工程與應(yīng)用(10)可見狀態(tài)方程和觀測方程都是線性的.3.2姿態(tài)角濾波方程本文采用角速率陀螺儀測量航向角或角速度,當(dāng)車輛翻滾或俯仰時會引入測量誤差,考慮車輛無翻滾情況,如果能夠精確測量到傾角,就可以采用補(bǔ)償算法.一對微機(jī)械加速度計(jì)就可以測量到傾角.其原理是加速度計(jì)不僅可以敏感載體運(yùn)動加速度,還可以敏感其安裝軸向上重力加速度分量.加速度計(jì)在短時間內(nèi)測量精度高,刻度系數(shù)穩(wěn)定,雖然隨著時間增長,加速度計(jì)測量的位置誤差隨時間成重

11、積分關(guān)系增長,但是加速度計(jì)用來測量傾角可直接利用加速度計(jì)輸出不需積分,其原理是iBm=sin(),因此角度測量誤差遠(yuǎn)遠(yuǎn)小于位置測量誤差.其中g(shù)0,gy,ar,0分別是重力加速度在y軸方向的分量,載體前進(jìn)的加速度和,軸加速度計(jì)的測量值.當(dāng)車輛處于靜止或勻速運(yùn)動情況下gc=a.姿態(tài)角濾波狀態(tài)變量:】狀態(tài)方程:X2(+1)=2()X2()+2()+'I,2()(11)一,t-J,U2(J):l一是控制項(xiàng).【co昌IJ觀測量為:zt.Ps"Psp觀測方程為:z2()=2(),k)+2()(12)lcosinlk(J),J)=hh=l.c.sl【J將h2()在的一步預(yù)報(bào)估值(J,J一1

12、)處泰勒展開:z(立)+l(13)()-X2(J,k-1)J+'7(J)線性化的觀測方程:Z2()=而2(J),2(J)+壚(J)+(J)(14)其中Hl.p(J):h(立(,k一1),)一立(J,k一1)4噪聲方差自適應(yīng)加權(quán)Kalman濾波算法式(14)中(k)是包含泰勒展開的高階項(xiàng)和量測噪聲項(xiàng),稱其為虛擬觀測噪聲.解決非線性系統(tǒng)狀態(tài)估計(jì)問題一般采用擴(kuò)展卡爾曼濾波(EKF)方法,考慮到實(shí)際的噪聲統(tǒng)計(jì)特性是未知的,而且非線性系統(tǒng)線性化過程中會引入模型誤差.以上情況都有可能引起濾波發(fā)散.因此本文提出了將量測的線性化誤差納入量測噪聲項(xiàng),采用擴(kuò)展卡爾曼濾波算法,在對系統(tǒng)狀態(tài)估計(jì)的同時,對時變

13、噪聲統(tǒng)計(jì)進(jìn)行估計(jì).形成兩段互耦的自適應(yīng)擴(kuò)展卡爾曼濾波算法.由于對俯仰角的觀測量由GPs和DR同時提供,當(dāng)口接近零時GPS很難對其提供有效的觀測信息,觀測量主要由加速度計(jì)提供,因此應(yīng)根據(jù)車輛的傾斜度情況來確定對加速度計(jì)信息加權(quán)值,本文通過在濾波過程中與口呈單調(diào)增函數(shù)的A來確定觀測方差陣峨的加權(quán).瞬(15)L=deg(1,l,A)(16)擴(kuò)展卡爾曼濾波方程:X(k+l,k)=(X(k)+ll(k)(17)X(k+1)=X(后+1,k)+置(后+1)e(k+1)(18)e(k+1)=Z(后+1)一【(后+l,k)卜r(k)(19)K(k+1)=尸(后+1,k)HT(后+1)(20)【日(后+1)P(

14、k+l,k)王(后+1)+足(后)】P(k+l,k)=(后)P(k)(后)+Q(后)(21)P(k+1)=【J(+1)H(k+1)lP(后+l,k)(22)時變噪聲估值方程:r(k+1)=(1-dk),(k)+dZ(后+1)-h(后+1)】(23)R(k+1)-kr(1一d)足()+d【8(后+1)(后+1)一(24)H(k)P(k+l,k)(后)】其中dk=(1一b)/(1一b¨),b是小于1的正數(shù)稱為遺忘因子.k是加權(quán)系數(shù).5仿真及試驗(yàn)結(jié)果為了驗(yàn)證算法.本文采用靜態(tài)測量數(shù)據(jù)和郊外跑車數(shù)據(jù)進(jìn)行半實(shí)物仿真.靜態(tài)測量時加速度計(jì)數(shù)據(jù)噪聲加大后,濾波效果仍然較好.見圖3.0_40.2.一0

15、?20.4o2ooo4ooo6ooo8ooot/s圖3靜態(tài)測量時俯仰角濾波結(jié)果東向位置/m圖4無姿態(tài)角校正的濾波結(jié)果帶姿態(tài)角濾波反饋校正的濾波結(jié)果如圖5.由于陀螺和加速度計(jì)的誤差DR單獨(dú)定位誤差加大.與GPS組合后,有效地抑制了誤差的累積,大大提高系統(tǒng)性能.通過38rain的跑車數(shù)據(jù)驗(yàn)證,單獨(dú)DR跑車時位置誤差隨時間積累.最大達(dá)到290m(見圖4中DR數(shù)據(jù)),在GPS數(shù)據(jù)正常時組合導(dǎo)航系統(tǒng)精度與GPS系統(tǒng)精度相當(dāng).當(dāng)1350s到1700s以及1900s到2300s時GPS數(shù)據(jù)丟失,無姿態(tài)角反饋校正的一般濾波算法結(jié)果圖4所示,最大位置誤差為270m:最大位置誤差為114m.可見在GPS失效的情況

16、下有姿態(tài)角反饋校正的DR系統(tǒng)推算精度明顯提高.刪_叵U東向位置圖5有姿態(tài)角反饋校正的濾波結(jié)果6結(jié)論本文以提高DR/GPS組合導(dǎo)航系統(tǒng)精度和可靠性為目標(biāo),提出了位置,姿態(tài)并行濾波的濾波器結(jié)構(gòu).進(jìn)而根據(jù)兩個子系統(tǒng)的特點(diǎn)應(yīng)用標(biāo)準(zhǔn)卡爾曼濾波算法和改進(jìn)的自適應(yīng)擴(kuò)展卡爾曼濾波算法,并為驗(yàn)證算法進(jìn)行了仿真試驗(yàn).該方法在保證組合導(dǎo)航系統(tǒng)定位精度的前提下,加強(qiáng)了對DR子系統(tǒng)的修正,提高了組合導(dǎo)航系統(tǒng)的可靠性和精度.(收稿日期:2oo5年6月)參考文獻(xiàn)1.鄭平方,常青.張其善.車載GPS/DR組合導(dǎo)航系統(tǒng)的研究【JJ.北京航空航天大學(xué),1999;25(5)2.莊朝文,富立,范躍祖.多傳感器融合技術(shù)在低成本車輛組合

17、導(dǎo)航定位系統(tǒng)中的應(yīng)用【JJ.中國慣性技術(shù),2000;10(5)3.ToshihiroAono.PositioningofvehicleonundulatinggroundusingGPSanddeadreckoningC.In:Procofthe1998IEEEInternationalConferenceonRobotics&AutomationWashington.1998054.Lanroojeda.JohannBorenstein.FLEXnav:FuzzyLosieEstimationExpeRule-basedPositionforMobileRobotsonRug

18、gedTerrainC.In:Procofthe2002IEEEInternationalConferenceonRobotics&AutomationWashington,USA,200205:3173225.鄧自立+自校正濾波理論及其應(yīng)reM.哈爾濱工業(yè)大學(xué)出版社,2003(上接210頁)參考文獻(xiàn)1.PanLiandong.HuangXinhan,MAfif.APC-basedopenarchitecturecontrollerforrobotJ】.InformationTechnologyJournal,2004;3(3):2963022.MCLin.JohnFCanny.AfastalgorithmsforinerementaldistancecomputationfC1.In:ProcIEEEConfonRoboticsandAutonkation,199l:lo08一lOl43.SEhmann,MCLin.Acceleratedproximityqueriesbetweenconvex