劉 娣,朱松青,陳 桂,朱震曙

(1.南京工程學(xué)院 自動(dòng)化學(xué)院,江蘇 南京 211167;2.江蘇金陵智造研究院有限公司,江蘇 南京 210006)

衛(wèi)星導(dǎo)航系統(tǒng)和慣性導(dǎo)航系統(tǒng)(Inertial navigation system,INS)由于高度的互補(bǔ)性,已經(jīng)成為很多載體獲取位置、速度和姿態(tài)信息的標(biāo)準(zhǔn)設(shè)備[1-5],如無(wú)人駕駛汽車使用衛(wèi)星/INS組合導(dǎo)航系統(tǒng)提供車道級(jí)定位[6],列車采用全球定位系統(tǒng)(Globe position system,GPS)和INS相結(jié)合實(shí)現(xiàn)高精度定位[7]。根據(jù)信息融合的層次,衛(wèi)星/INS組合導(dǎo)航系統(tǒng)可以歸納為松組合、緊組合和深組合導(dǎo)航系統(tǒng)[8,9]。松組合導(dǎo)航系統(tǒng)構(gòu)建導(dǎo)航濾波器量測(cè)矩陣時(shí),利用衛(wèi)星導(dǎo)航系統(tǒng)和INS輸出的位置和速度信息[9];緊組合導(dǎo)航系統(tǒng)采用偽距和偽距率差值構(gòu)建量測(cè)矩陣,相對(duì)松組合系統(tǒng)而言,利用了更深層次的信息[10];深組合導(dǎo)航系統(tǒng)在緊組合系統(tǒng)的基礎(chǔ)上增加了利用組合導(dǎo)航信息輔助衛(wèi)星信號(hào)跟蹤[11]。松組合導(dǎo)航系統(tǒng)沒(méi)有涉及導(dǎo)航系統(tǒng)內(nèi)部,僅單獨(dú)使用衛(wèi)星導(dǎo)航系統(tǒng)和INS輸出的位置和速度信息,實(shí)現(xiàn)簡(jiǎn)單,計(jì)算量小。與松組合導(dǎo)航系統(tǒng)相比,緊組合和深組合系統(tǒng)采用偽距/偽距率構(gòu)建量測(cè)信息,因此量測(cè)矩陣的維數(shù)和參與解算的衛(wèi)星有關(guān),更多的衛(wèi)星參與解算會(huì)導(dǎo)致計(jì)算量“爆炸”。尤其是多星座系統(tǒng),可能會(huì)有幾十顆衛(wèi)星參與解算,對(duì)于實(shí)時(shí)估計(jì)場(chǎng)景,需要在幾ms以內(nèi)完成組合導(dǎo)航濾波器的解算,這在量測(cè)矩陣維數(shù)爆炸的情況下是不可能完成的。

為了減少深組合導(dǎo)航系統(tǒng)計(jì)算量,通常采用聯(lián)邦濾波的方法。Xie等[12,13]提出一種基于聯(lián)邦濾波的方法,將每個(gè)星座對(duì)應(yīng)的組合導(dǎo)航系統(tǒng)單獨(dú)作為1個(gè)子濾波器處理,然后設(shè)置1個(gè)主濾波器完成信息的融合。然而每個(gè)子濾波器計(jì)算量并沒(méi)有減少,單個(gè)星座的衛(wèi)星數(shù)的增加會(huì)導(dǎo)致濾波器計(jì)算量增加。Luo等[14]提出一種預(yù)濾波的方法,利用多個(gè)并行的低維數(shù)濾波器來(lái)進(jìn)行預(yù)處理,這樣可以降低組合導(dǎo)航濾波器的更新頻率,減少計(jì)算量,然而對(duì)于一些需要高速更新導(dǎo)航信息的場(chǎng)景,降低更新率具有一定的局限性。為了減少多星座衛(wèi)星/INS深組合導(dǎo)航系統(tǒng)的計(jì)算量,本文以GPS/北斗衛(wèi)星導(dǎo)航系統(tǒng)(Beidou navigation satellite system,BDS)/INS基于偽距/偽距率的深組合導(dǎo)航系統(tǒng)為例,提出一種降維的分散式組合導(dǎo)航濾波方法,在保證導(dǎo)航定位精度的同時(shí),減少計(jì)算量,提高實(shí)時(shí)性。首先通過(guò)采用同一導(dǎo)航系統(tǒng)各通道之間的衛(wèi)星偽距差分和偽距率差分的方法,消除時(shí)鐘誤差相關(guān)狀態(tài)量,有效降低狀態(tài)方程維數(shù);再通過(guò)構(gòu)建聯(lián)邦濾波,將濾波方程分為2個(gè)子濾波器,降低量測(cè)矩陣維數(shù);通過(guò)子濾波器的并行運(yùn)行有效減少計(jì)算量。

1 集中式深組合系統(tǒng)模型

在單星座衛(wèi)星/INS組合導(dǎo)航系統(tǒng)中,通常以衛(wèi)星導(dǎo)航系統(tǒng)時(shí)鐘誤差和INS誤差構(gòu)建狀態(tài)方程,以2個(gè)導(dǎo)航系統(tǒng)之間的偽距/偽距率差構(gòu)建量測(cè)方程。當(dāng)增加衛(wèi)星星座時(shí),最簡(jiǎn)單的處理方法是集中式組合方法(Centralized integration method,CIM),即在組合濾波的狀態(tài)方程中增加相應(yīng)的時(shí)鐘相關(guān)狀態(tài)量,在組合濾波的量測(cè)方程中增加相應(yīng)的偽距/偽距率量測(cè)信息,通過(guò)主濾波器處理并估計(jì)狀態(tài),實(shí)現(xiàn)對(duì)慣性導(dǎo)航系統(tǒng)的誤差補(bǔ)償,達(dá)到抑制慣性導(dǎo)航系統(tǒng)發(fā)散的目的。

1.1 系統(tǒng)狀態(tài)方程

在慣性導(dǎo)航系統(tǒng)誤差基礎(chǔ)上建立多星座衛(wèi)星/INS深組合導(dǎo)航系統(tǒng)的狀態(tài)方程,狀態(tài)量增加了衛(wèi)星導(dǎo)航系統(tǒng)的時(shí)鐘誤差項(xiàng),包含系統(tǒng)時(shí)鐘誤差項(xiàng)的19維狀態(tài)變量為X=[XIXGPSXBDS]T,XI為慣性導(dǎo)航系統(tǒng)對(duì)應(yīng)的狀態(tài)誤差變量,XGPS和XBDS為GPS和BDS對(duì)應(yīng)的時(shí)鐘誤差,XI、XGPS、XBDS具體形式如下

式中:φE、φN、φU為東、北、天姿態(tài)誤差角;δvE、δvN、δvU為東、北、天速度誤差;δL、δλ、δh為緯度、經(jīng)度和高度誤差;εx、εy、εz和▽x、▽y、▽z分別為慣性器件在載體坐標(biāo)系下的陀螺隨機(jī)漂移、加速度計(jì)偏置;δtb,GPS為GPS接收機(jī)時(shí)鐘偏差;δtd,GPS為GPS接收機(jī)漂移;δtb,BDS為BDS接收機(jī)時(shí)鐘偏差;δtd,BDS為BDS接收機(jī)漂移。

多星座GPS/BDS/INS組合濾波方程中的狀態(tài)方程可表示為

(1)

即

(2)

式中:F為19×19階的系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣,G為19×10階的系統(tǒng)噪聲驅(qū)動(dòng)陣,W為12維噪聲向量。

1.2 系統(tǒng)量測(cè)方程

以GPS系統(tǒng)為例,偽距觀測(cè)方程可表示為

(3)

式中

系統(tǒng)偽距率觀測(cè)方程為

(4)

式中

(5)

eij為載體和對(duì)應(yīng)的衛(wèi)星之間的視線矢量;λ為經(jīng)度;L為緯度。

BDS觀測(cè)方程和GPS類似,綜合式(3)～(5),可以得到多星座GPS/BDS/INS組合導(dǎo)航系統(tǒng)量測(cè)方程為

(6)

2 分散式組合導(dǎo)航方法

2.1 分散式濾波系統(tǒng)模型

在集中式組合導(dǎo)航濾波中,時(shí)鐘誤差量是每個(gè)衛(wèi)星導(dǎo)航系統(tǒng)特有的,本文統(tǒng)一將衛(wèi)星導(dǎo)航系統(tǒng)2個(gè)通道的偽距/偽距率量測(cè)值進(jìn)行一次差分,抵消鐘差和鐘漂誤差項(xiàng)的影響,達(dá)到減少狀態(tài)方程維數(shù)的目的。

(1)基于偽距/偽距率差分的組合濾波方法狀態(tài)方程。

式(1)中集中式深組合系統(tǒng)模型狀態(tài)變量維數(shù)為19。相對(duì)于集中式方法,新模型的狀態(tài)方程中去除了時(shí)鐘相關(guān)誤差項(xiàng),狀態(tài)向量為

δhεxεyεz▽x▽y▽z]T

系統(tǒng)狀態(tài)方程如下

(7)

(2)基于偽距/偽距率差分的組合濾波方法量測(cè)方程。

(8)

(9)

偽距差分方程

(10)

偽距差分觀測(cè)方程為

(11)

式中

i=2,3,…,n;j=1,2,3

偽距率差分方程

(12)

偽距率差分觀測(cè)方程為

(13)

式中

(14)

綜上,新的觀測(cè)方程可表示為

(15)

2.2 分散式組合導(dǎo)航方法分析

2.1節(jié)通過(guò)星間差分消除了時(shí)鐘相關(guān)變量,降低了狀態(tài)方程和量測(cè)方程的維數(shù),本節(jié)進(jìn)一步研究將濾波器分拆成多個(gè)子濾波器,通過(guò)分散估計(jì)、兩步級(jí)聯(lián)的方法減少計(jì)算量,即分散式組合方法(Decentralized integration method,DIM)。圖1所示為聯(lián)邦卡爾曼濾波器結(jié)構(gòu)框圖,其中包含GPS/INS組合子濾波器和BDS/INS組合子濾波器2個(gè)并行的子濾波器,兩者均為卡爾曼濾波器,子濾波器同步向主濾波器提供局部估計(jì)信息;主濾波器實(shí)現(xiàn)對(duì)子濾波器狀態(tài)估計(jì)信息的全局融合,并將結(jié)果反饋給各子濾波器。

圖1 聯(lián)邦卡爾曼濾波器結(jié)構(gòu)框圖

(1)GPS/INS組合子濾波器。

(16)

(17)

(2)BDS/INS組合子濾波器。

(18)

(19)

(3)主濾波器。

信息分配方法具體為:系統(tǒng)信息在主、子濾波器間的分配滿足

βm+βGPS+βBDS=1

(20)

式中:βm、βGPS、βBDS分別為主、子濾波器信息分配系數(shù)。本文研究的分散式系統(tǒng)聯(lián)邦濾波器中,主濾波器不進(jìn)行信息分配,主濾波器僅對(duì)子濾波器估計(jì)信息進(jìn)行全局融合并對(duì)子濾波器進(jìn)行反饋,即

(21)

GPS/INS子濾波器和BDS/INS子濾波器信息分配系數(shù)陣βGPS和βBDS具體形式如下

(22)

(23)

系統(tǒng)信息在子濾波器中的具體分配方法如下

(24)

主濾波器數(shù)據(jù)融合方法具體如下

(25)

3 系統(tǒng)仿真和算法性能分析

3.1 仿真實(shí)驗(yàn)

本文以衛(wèi)星導(dǎo)航系統(tǒng)模擬器為信號(hào)源,對(duì)本文算法進(jìn)行仿真分析,并與常規(guī)CIM方法進(jìn)行性能對(duì)比。首先由衛(wèi)星導(dǎo)航系統(tǒng)模擬器產(chǎn)生相應(yīng)的GPS/BDS導(dǎo)航衛(wèi)星信號(hào),然后對(duì)中頻信號(hào)數(shù)據(jù)進(jìn)行采集,再由軟件接收機(jī)對(duì)中頻信號(hào)數(shù)據(jù)進(jìn)行處理。軌跡數(shù)據(jù)來(lái)自于軌跡發(fā)生器。軌跡發(fā)生器能夠模擬產(chǎn)生包括比力、陀螺儀角速率、位置、速度和姿態(tài)信息在內(nèi)的慣性數(shù)據(jù)。構(gòu)建多星座衛(wèi)星/INS深組合導(dǎo)航仿真系統(tǒng),對(duì)算法進(jìn)行仿真分析。系統(tǒng)相關(guān)參數(shù)如表1所示,偽距、偽距率觀測(cè)白噪聲分別為15 m和0.1 m/s,組合導(dǎo)航卡爾曼濾波周期為1 s。

表1 組合導(dǎo)航系統(tǒng)參數(shù)表

仿真結(jié)果如圖2～圖4所示。圖2表示單星座GPS/INS深組合導(dǎo)航系統(tǒng)、多星座GPS/BDS/INS深組合導(dǎo)航系統(tǒng)DIM方法和CIM方法在緯度、經(jīng)度和高度的位置誤差。表2為位置誤差的均方根誤差、方差以及平均值分析結(jié)果。分析可知,相比于單星座GPS/INS深組合導(dǎo)航系統(tǒng),多星座GPS/BDS/INS深組合導(dǎo)航系統(tǒng)DIM方法具有更高的精度,其緯度、經(jīng)度和高度的均方根誤差分別減少了44.94%、27.27%和59.29%,方差分別減少了75%、47.44%和83.16%,平均值分別減少了37.17%、48.48%和57.39%。相比于CIM方法,DIM方法緯度、經(jīng)度和高度的均方根誤差分別減少了19.67%、5.88%和32.94%,方差分別減少了60%、10.87%和54.79%,平均值分別減少了22.83%、12.82%和11.76%。DIM方法緯度、經(jīng)度和高度的位置精度相對(duì)于CIM方法有一定的提升。

圖2 位置誤差曲線圖

表2 位置誤差數(shù)據(jù)統(tǒng)計(jì)分析表

圖3為GPS/INS深組合導(dǎo)航系統(tǒng)、GPS/BDS/INS深組合導(dǎo)航系統(tǒng)DIM方法和CIM方法的東速、北速和天速誤差曲線。表3為速度均方根誤差、方差以及平均值分析結(jié)果。分析可知,相比于單星座GPS/INS深組合導(dǎo)航系統(tǒng),多星座GPS/BDS/INS深組合導(dǎo)航系統(tǒng)DIM方法具有更高的精度,其東速、北速和天速均方根誤差分別減少了23.53%、23.53%和16.67%,方差分別減少了23.81%、44.83%和22.22%,平均值分別減少了-11.18%、-17.65%和17.39%。相比于CIM方法,DIM方法東速、北速和天速的均方根誤差分別減少了0%、7.14%和7.41%,方差分別減少了0%、5.88%和-14.29%,平均值分別減少了3.01%、0%和9.52%。DIM方法東速、北速和天速精度與CIM方法相當(dāng)。

圖3 速度誤差曲線圖

圖4為GPS/INS深組合導(dǎo)航系統(tǒng)、GPS/BDS/INS慣性深組合導(dǎo)航系統(tǒng)DIM方法和CIM方法偏航角、俯仰角和滾轉(zhuǎn)角誤差曲線,誤差分析結(jié)果如表4所示。分析可知,相比于單星座GPS/INS深組合導(dǎo)航系統(tǒng),多星座GPS/BDS/INS深組合導(dǎo)航系統(tǒng)DIM方法具有更高的精度,其偏航角、俯仰角和滾轉(zhuǎn)角均方根誤差分別減少了59.26%、57.69%和40.74%,方差分別減少了77.36%、57.14%和23.94%,平均值分別減少了68.75%、62.50%和16.22%。相比于CIM方法,DIM方法偏航角、俯仰角和滾轉(zhuǎn)角均方根誤差分別減少了21.43%、-9.09%和30.43%,方差分別減少了40%、-8.33%和-50%,平均值分別減少了41.18%、-8.33%和-3.23%。DIM方法偏航角、俯仰角和滾轉(zhuǎn)角精度與CIM方法相當(dāng)。

圖4 姿態(tài)誤差曲線圖

表3 速度誤差數(shù)據(jù)統(tǒng)計(jì)分析表

表4 姿態(tài)誤差數(shù)據(jù)統(tǒng)計(jì)分析表

綜合上述分析結(jié)果可看出,相對(duì)于單星座系統(tǒng),本文多星座GPS/BDS/INS深組合導(dǎo)航系統(tǒng)精度有明顯提高,DIM相比于CIM精度也有一定程度的提升,但由于分散式濾波采用分散估計(jì)、兩步級(jí)聯(lián)的分散化濾波方法,能有效降低計(jì)算量。

3.2 算法復(fù)雜度和計(jì)算量分析

根據(jù)3.1節(jié)分析可知,本文提出的多星座GPS/BDS/INS深組合導(dǎo)航系統(tǒng)DIM方法精度相對(duì)CIM方法精度有一定的提升,本節(jié)在Alienware筆記本(I7 CPU,16GB RAM)平臺(tái)上,基于MATLAB2018軟件仿真運(yùn)行相關(guān)方法,對(duì)DIM方法和CIM方法的計(jì)算復(fù)雜度和計(jì)算量進(jìn)行進(jìn)一步分析。本文利用星間差分和聯(lián)邦濾波的方法,將量測(cè)方程拆分成2部分分別計(jì)算。假設(shè)GPS和BDS可見(jiàn)星數(shù)目分別是m和n,表5列出了CIM導(dǎo)航濾波器、GPS/INS子濾波器、BDS/INS子濾波器相關(guān)矩陣維數(shù)變化的關(guān)系,可見(jiàn)相對(duì)于CIM導(dǎo)航濾波器,后兩者的觀測(cè)矩陣維數(shù)、觀測(cè)方差陣維數(shù)、卡爾曼濾波增益矩陣維數(shù)都大大降低,由于卡爾曼濾波算法計(jì)算量和矩陣維數(shù)呈指數(shù)級(jí)關(guān)系,因此隨著相關(guān)矩陣維數(shù)的增加,算法計(jì)算量將快速增加。濾波器100次仿真單次平均運(yùn)行時(shí)間如表5所示,可以看出,當(dāng)GPS和BDS可見(jiàn)星數(shù)目均為4時(shí),DIM運(yùn)行時(shí)間相對(duì)于CIM減少了大約31.7%;當(dāng)GPS和BDS可見(jiàn)星數(shù)目均為5時(shí),DIM運(yùn)行時(shí)間相對(duì)于CIM減少了大約28.4%。這一方面是因?yàn)镈IM相關(guān)矩陣維數(shù)較低,有效減小了計(jì)算量;另一方面,DIM的2個(gè)子濾波器可以并行運(yùn)行,可有效減少運(yùn)行時(shí)間。因此,DIM方法能在保證定位精度的前提下有效提高運(yùn)行速度,更好地滿足實(shí)時(shí)性要求高的應(yīng)用場(chǎng)合。

表5 算法復(fù)雜度和計(jì)算量分析結(jié)果表

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

本文提出了一種基于星間差分和聯(lián)邦濾波的分散式組合濾波器架構(gòu),解決了多星座GPS/BDS/INS深組合導(dǎo)航系統(tǒng)組合導(dǎo)航濾波器計(jì)算量大、實(shí)時(shí)性差的問(wèn)題。不同于集中式組合導(dǎo)航濾波方法,本文在通過(guò)星間差分降低狀態(tài)方程維數(shù)的基礎(chǔ)上,進(jìn)一步通過(guò)分散式濾波方法降低了量測(cè)方程的維數(shù),有效減少了計(jì)算量。最后以GPS/BDS/INS深組合導(dǎo)航系統(tǒng)為例進(jìn)行了仿真分析。實(shí)驗(yàn)結(jié)果表明,本文方法可以在保持定位精度的同時(shí)顯著減少計(jì)算量,滿足實(shí)時(shí)導(dǎo)航應(yīng)用場(chǎng)景需求。