多傳感器異步信息多源組合導(dǎo)航算法實(shí)現(xiàn)

趙海飛,陳 帥,劉清秀,廖澤俁,馬永犇

（南京理工大學(xué)自動化學(xué)院,南京 210094)

0 引言

目前,導(dǎo)航領(lǐng)域的研究以組合導(dǎo)航為主,尤其是以慣性導(dǎo)航為基的多源組合導(dǎo)航。聯(lián)邦Kalman濾波要求各個傳感器的量測信息同時到達(dá)[1],但是多源組合導(dǎo)航系統(tǒng)中傳感器數(shù)量和種類較多,各傳感器的采樣率存在差異,導(dǎo)致各子濾波器的量測周期不同,即異質(zhì)傳感器異步信息的不等間隔問題,影響了系統(tǒng)的導(dǎo)航精度[2]。

秦永元等[3-4]設(shè)計(jì)了一種經(jīng)典的異步信息融合方法,利用時間更新將來自不同傳感器的異步信息進(jìn)行同步。劉麗麗等[5]基于多尺度模型設(shè)計(jì)了一種多傳感器組合導(dǎo)航系統(tǒng),將異步融合算法分為時間序貫過程和傳感器量測序慣過程,仿真結(jié)果證明了該算法的高精度性和可行性,但是該算法計(jì)算量大,工程實(shí)現(xiàn)困難。王宏等[6]設(shè)計(jì)了一種異步聯(lián)邦無跡Kalman濾波器（Unscented Kalman Filter,UKF),將濾波器中的時間更新與量測更新分離,以此來解決全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System,GNSS)/捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System,SINS)/攝影測量（Photogrammetry)組合導(dǎo)航系統(tǒng)測量量測信息不等間隔問題。

本文簡化了文獻(xiàn)[6]的算法,設(shè)計(jì)了一種異質(zhì)傳感器異步信息非等間隔濾波算法,以INS/GNSS/高度計(jì)/磁強(qiáng)計(jì)組合導(dǎo)航系統(tǒng)為例,通過半物理仿真實(shí)驗(yàn)驗(yàn)證了該算法的可行性和導(dǎo)航精度,并將其移植到嵌入式平臺,通過跑車實(shí)驗(yàn)證明該算法能夠工程實(shí)現(xiàn),有較高的工程應(yīng)用價值。

1 聯(lián)邦濾波器設(shè)計(jì)

Kalman濾波主要分為集中式Kalman濾波與分布式Kalman濾波兩種方式。集中式Kalman濾波雖然能夠得到對狀態(tài)的最優(yōu)估計(jì),但由于傳感器種類的增加以及濾波狀態(tài)維數(shù)的增大,使得集中式Kalman濾波的計(jì)算負(fù)擔(dān)和通信負(fù)擔(dān)加重,不易于工程實(shí)現(xiàn)[7]。此外,當(dāng)某個導(dǎo)航傳感器發(fā)生故障而導(dǎo)致其導(dǎo)航精度下降甚至發(fā)散時,集中式Kalman濾波的結(jié)構(gòu)特性使得系統(tǒng)無法對故障信息源進(jìn)行檢測與隔離,系統(tǒng)導(dǎo)航結(jié)果整體精度也會隨之下降,系統(tǒng)的容錯性能差。為了解決以上問題,Carlson于1988年提出了聯(lián)邦Kalman濾波（Federated Kalman Filtering)。相對于集中式Kalman濾波,聯(lián)邦Kalman濾波降低了算法的復(fù)雜性、提高了容錯性和可靠性,并且設(shè)計(jì)靈活,便于工程實(shí)現(xiàn),受到了廣泛的重視[8-9]。

聯(lián)邦Kalman濾波是一種具有兩級結(jié)構(gòu)的分布式Kamlan濾波,該濾波算法由若干個子濾波器和一個主濾波器組成,其結(jié)構(gòu)如圖1所示。

圖1 聯(lián)邦濾波結(jié)構(gòu)示意圖Fig.1 Structure diagram of federated filtering

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

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

式（1)中,X（t)為由SINS誤差變量構(gòu)成的18維狀態(tài)向量,F（t)為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣,G（t)為系統(tǒng)的噪聲驅(qū)動矩陣,W（t)為系統(tǒng)的噪聲隨機(jī)誤差向量。

系統(tǒng)的狀態(tài)向量設(shè)計(jì)為

式（2)中, ?e、 ?n、 ?u分別為捷聯(lián)式慣性導(dǎo)航系統(tǒng)在東向、北向、天向上的姿態(tài)角誤差,δVe、δVn、δVu分別為捷聯(lián)式慣性導(dǎo)航系統(tǒng)在東向、北向、天向上的速度誤差,δL、δλ、δh分別為捷聯(lián)式慣性導(dǎo)航系統(tǒng)在緯度、經(jīng)度、高度上的位置誤差, εbx、 εby、 εbz分別為陀螺儀在 x、 y、 z三個軸向上的隨機(jī)常值誤差,εrx、εry、εrz分別為陀螺儀在x、y、z三個軸向上的一階 Markov偏移誤差,、、分別為加速度計(jì)在x、y、z三個軸向上的一階Markov偏移誤差[10]。

1.2 量測方程

子濾波器1將SINS輸出的位置、速度量測值與GPS輸出的位置、速度量測值作差作為系統(tǒng)量測向量,系統(tǒng)量測方程為

式（3)中,Ls、 λs、 hs為 SINS 輸出的緯度、 經(jīng)度、高度信息,Vse、Vsn、Vsu為SINS輸出的地理系下載體在 x、y、z三個軸向上的速度信息,Lg、λg、hg為 GNSS輸出的緯度、經(jīng)度、高度信息,Vge、Vgn、Vgu為 GNSS輸出的地理系下載體在 x、y、z三個軸向上的速度信息,H1（t)為量測矩陣,V1（t)為量測噪聲。

子濾波器2將SINS輸出的高度量測值與高度計(jì)輸出的高度量測值作差作為系統(tǒng)量測向量,系統(tǒng)量測方程為

式（4)中,ha為高度計(jì)輸出的高度信息,H2（t)為量測矩陣,V2（t)為量測噪聲。

子濾波器3將SINS輸出的航向角量測值與磁強(qiáng)計(jì)輸出的航向角量測值作差作為系統(tǒng)量測向量,系統(tǒng)量測方程為

式（5)中,ψs為 SINS輸出的航向角信息,ψc為磁強(qiáng)計(jì)輸出的航向角信息,H3（t)為量測矩陣,V3（t)為量測噪聲。

1.3 主濾波器信息融合

主濾波器將各子濾波器的濾波結(jié)果進(jìn)行融合,融合方法如下

式（6)中,Pg、為主濾波器輸出的最優(yōu)方差陣和全局最優(yōu)估計(jì)。

2 異質(zhì)異步非等間隔信息處理

2.1 算法設(shè)計(jì)

首先,分離Kalman濾波器中的時間更新和量測更新。時間更新為

設(shè)第 i個傳感器的量測周期為 Ti（i=1,2,3,…,m),主濾波器的計(jì)算周期為Tc,融合周期為Tf,Tc和Tf定義如下

式（9)中,G（·)表示求取最大公約數(shù),L（·)表示求取最小公倍數(shù)。定義m為各計(jì)算周期Tc劃分的時間刻度,有Ti=miTc（mi為互質(zhì)的自然數(shù))。當(dāng)各子濾波器同時有量測更新時,即Tf=Tc時,完全滿足聯(lián)邦Kalman濾波算法要求;否則,設(shè)計(jì)如下算法:

（1)對子濾波器

當(dāng)m=mp時,即此時主濾波器有量測更新時

當(dāng)m≠mp時,即此時主濾波器沒有量測更新時,子濾波器只進(jìn)行式（7)的時間更新。

（2)對主濾波器

當(dāng)Tf=Tc,即此時主濾波器有量測更新時,主濾波器進(jìn)行式（6)的信息融合;當(dāng)Tf≠Tc時,即此時主濾波器沒有量測更新時,此時主濾波器不進(jìn)行任何操作。

其中,mp為子濾波器有新的量測更新的時刻,如果當(dāng)前子濾波器沒有新的量測信息時,即滿足m≠mp時,子濾波器只進(jìn)行時間更新;如果當(dāng)前子濾波器有新的量測信息時,即滿足m=mp,子濾波器同時進(jìn)行時間更新和量測更新。對于主濾波器,只有當(dāng)主濾波器有量測更新時,主濾波器進(jìn)行信息分配。

2.2 嵌入式平臺搭建

本文選擇國產(chǎn)自主研發(fā)設(shè)計(jì)制造的MIMU（微型慣性測量單元)模塊、北斗導(dǎo)航計(jì)算機(jī)和芯片原子鐘深度集成一體化,通過串口與上位機(jī)實(shí)現(xiàn)通信,實(shí)時查看導(dǎo)航結(jié)果并保存數(shù)據(jù)。其中,MIMU模塊陀螺儀零偏穩(wěn)定性≤3（°)/h,加速度計(jì)零偏穩(wěn)定性≤0.25mg,原子鐘1pps同步精度優(yōu)于±50ns,外秒馴服精度等級和頻率調(diào)節(jié)范圍等級分別可達(dá)5×10-12和1×10-6,原子鐘將BDS導(dǎo)航計(jì)算機(jī)解算出的1pps信號進(jìn)行同步馴服,并輸出馴服后的1pps信號為外界用戶提供授時服務(wù)。外源傳感器選擇低成本氣壓高度計(jì)和磁強(qiáng)計(jì),通過CAN總線接入嵌入式平臺實(shí)現(xiàn)即插即用。

3 仿真校驗(yàn)

為了驗(yàn)證所述的異質(zhì)異步非等間隔信息處理算法在實(shí)際工程應(yīng)用中的可靠性和有效性,本文設(shè)計(jì)了半物理仿真實(shí)驗(yàn)。通過實(shí)際跑車采集MIMU、北斗衛(wèi)星接收機(jī)、高度計(jì)和磁強(qiáng)計(jì)的數(shù)據(jù)進(jìn)行離線仿真,并與SPAN-KVH1750高精度組合導(dǎo)航系統(tǒng)采集的基準(zhǔn)數(shù)據(jù)進(jìn)行比較。跑車實(shí)驗(yàn)初始位置為北緯32.016047°、東經(jīng)118.889261°,高度為7m,尋北結(jié)果為-24.633°。設(shè)備裝車效果圖如圖2所示,跑車行駛路線如圖3所示。

圖2 設(shè)備裝車效果圖Fig.2 Effect diagram of equipment loading

圖3 跑車實(shí)驗(yàn)行駛路線Fig.3 Driving route of vehicle experiment

上述設(shè)備的性能指標(biāo)如表1所示。

由表1可知,實(shí)驗(yàn)所用傳感器輸出頻率各不相同,必然會導(dǎo)致異質(zhì)傳感器異步信息的不等間隔問題。

表1 實(shí)驗(yàn)設(shè)備性能指標(biāo)Table 1 Performance indicators of test equipment

跑車時長為300s,得到的數(shù)據(jù)通過上位機(jī)監(jiān)控存入計(jì)算機(jī),然后分別進(jìn)行異質(zhì)異步非等間隔信息處理算法和傳統(tǒng)分散式聯(lián)邦濾波算法數(shù)據(jù)融合離線仿真,通過與高精度基準(zhǔn)對比得出誤差,兩者的誤差對比曲線如圖4、圖5所示。

實(shí)驗(yàn)過程中為保證本文算法正常運(yùn)行,數(shù)據(jù)處理時截取了車輛行駛過程中各傳感器均正常工作時的一段數(shù)據(jù),剔除了傳感器故障時的數(shù)據(jù)（如衛(wèi)導(dǎo)丟星、PDOP值過高等),故導(dǎo)致部分方向上初始誤差不同。由圖4可知,使用聯(lián)邦濾波算法時,經(jīng)計(jì)算得到的東向速度的均方根誤差為0.1272m/s,北向速度的均方根誤差為0.1300m/s,天向速度的均方根誤差為0.1013m/s;使用非等間隔信息處理算法后,經(jīng)計(jì)算得到的東向速度的均方根誤差為0.0939m/s,北向速度的均方根誤差為0.0919m/s,天向速度的均方根誤差為0.0930m/s,東北天速度精度分別提升了26.2%、29.3%和8.2%。由圖5可知,使用聯(lián)邦濾波算法時,經(jīng)計(jì)算得到的緯度方向的均方根誤差為2.1193m,經(jīng)度方向的均方根誤差為4.3058m,高度方向的均方根誤差為6.2032m;使用非等間隔信息處理算法后,經(jīng)計(jì)算得到的緯度方向的均方根誤差為1.3842m,經(jīng)度方向的均方根誤差為3.3030m,高度方向的均方根誤差為5.4140m,東北天位置精度分別提升了34.7%、23.3%和12.7%。

圖4 速度誤差對比曲線Fig.4 Comparison curves of velocity error

圖5 位置誤差對比曲線Fig.5 Comparison curves of position error

上述仿真結(jié)果表明,添加非等間隔信息處理后確實(shí)能夠提高導(dǎo)航性能,輸出更加精確、可靠的導(dǎo)航結(jié)果。

4 跑車實(shí)驗(yàn)

圖6為跑車實(shí)驗(yàn)路線圖,跑車初始位置與初始航向設(shè)置與仿真校驗(yàn)相同,設(shè)備裝車效果和設(shè)備性能指標(biāo)也和仿真校驗(yàn)一致（圖2和表1)。

圖6 跑車實(shí)驗(yàn)路線圖Fig.6 Roadmap of vehicle experiment

本次跑車實(shí)驗(yàn)先給導(dǎo)航板上電正常啟動,等待至接收機(jī)定位且進(jìn)入組合導(dǎo)航階段,輸出穩(wěn)定后啟動車輛進(jìn)行跑車測試。本次測試選用NovAtel-KVH1750作為參考系統(tǒng),測試得到的數(shù)據(jù)與其進(jìn)行速度、位置對比,結(jié)果如圖7、圖8所示。

圖7 速度誤差對比結(jié)果Fig.7 Comparison results of velocity error

圖8 位置誤差對比結(jié)果Fig.8 Comparison results of position error

實(shí)驗(yàn)過程中,部分方向上初始誤差不同是因?yàn)榻厝×烁鱾鞲衅骶９ぷ鲿r的一段數(shù)據(jù)。由圖7可知,使用聯(lián)邦濾波算法時,經(jīng)計(jì)算得到的東向速度的均方根誤差為0.1461m/s,北向速度的均方根誤差為0.2355m/s,天向速度的均方根誤差為0.3399m/s;使用非等間隔信息處理算法后,經(jīng)計(jì)算得到的東向速度的均方根誤差為0.1007m/s,北向速度的均方根誤差為0.1544m/s,天向速度的均方根誤差為0.1201m/s,東北天速度精度分別提升了31.1%、34.4%和64.6%。由圖8可知,使用聯(lián)邦濾波算法時,經(jīng)計(jì)算得到的緯度方向的均方根誤差為3.5225m,經(jīng)度方向的均方根誤差為2.5570m,高度方向的均方根誤差為2.1985m;使用非等間隔信息處理算法后,經(jīng)計(jì)算得到的緯度方向的均方根誤差為2.3899m,經(jīng)度方向的均方根誤差為1.5371m,高度方向的均方根誤差為1.2726m,東北天位置精度分別提升了32.2%、39.9%和42.1%。

由跑車實(shí)驗(yàn)測試結(jié)果可知,加入非等間隔濾波算法后,位置精度和速度精度都有較大提升,證明了異質(zhì)異步非等間隔信息處理算法切實(shí)有效,應(yīng)用于組合導(dǎo)航算法后能有效提高導(dǎo)航精度,具有較高的工程應(yīng)用價值。

5 結(jié)論

本文詳細(xì)介紹了異質(zhì)異步非等間隔信息處理算法,通過將Kalman濾波的時間更新和量測更新分離,有效解決了不等間隔問題。以車載SINS/GNSS/高度計(jì)/磁強(qiáng)計(jì)多源組合導(dǎo)航系統(tǒng)為實(shí)驗(yàn)對象,分別進(jìn)行了半物理仿真和跑車實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明,異質(zhì)異步非等間隔信息處理算法在多源組合導(dǎo)航系統(tǒng)中能夠解決不同傳感器量測信息不同步的問題,有效提高了系統(tǒng)位置精度和速度精度。該算法可以應(yīng)用到工程實(shí)踐中,具有很高的應(yīng)用價值。