多傳感器異步信息多源組合導航算法實現

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

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

0 引言

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

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

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

1 聯邦濾波器設計

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

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

圖1 聯邦濾波結構示意圖Fig.1 Structure diagram of federated filtering

1.1 系統狀態方程

系統的狀態方程為

式（1)中,X（t)為由SINS誤差變量構成的18維狀態向量,F（t)為系統的狀態轉移矩陣,G（t)為系統的噪聲驅動矩陣,W（t)為系統的噪聲隨機誤差向量。

系統的狀態向量設計為

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

1.2 量測方程

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

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

子濾波器2將SINS輸出的高度量測值與高度計輸出的高度量測值作差作為系統量測向量,系統量測方程為

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

子濾波器3將SINS輸出的航向角量測值與磁強計輸出的航向角量測值作差作為系統量測向量,系統量測方程為

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

1.3 主濾波器信息融合

主濾波器將各子濾波器的濾波結果進行融合,融合方法如下

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

2 異質異步非等間隔信息處理

2.1 算法設計

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

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

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

（1)對子濾波器

當m=mp時,即此時主濾波器有量測更新時

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

（2)對主濾波器

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

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

2.2 嵌入式平臺搭建

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

3 仿真校驗

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

圖2 設備裝車效果圖Fig.2 Effect diagram of equipment loading

圖3 跑車實驗行駛路線Fig.3 Driving route of vehicle experiment

上述設備的性能指標如表1所示。

由表1可知,實驗所用傳感器輸出頻率各不相同,必然會導致異質傳感器異步信息的不等間隔問題。

表1 實驗設備性能指標Table 1 Performance indicators of test equipment

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

實驗過程中為保證本文算法正常運行,數據處理時截取了車輛行駛過程中各傳感器均正常工作時的一段數據,剔除了傳感器故障時的數據（如衛導丟星、PDOP值過高等),故導致部分方向上初始誤差不同。由圖4可知,使用聯邦濾波算法時,經計算得到的東向速度的均方根誤差為0.1272m/s,北向速度的均方根誤差為0.1300m/s,天向速度的均方根誤差為0.1013m/s;使用非等間隔信息處理算法后,經計算得到的東向速度的均方根誤差為0.0939m/s,北向速度的均方根誤差為0.0919m/s,天向速度的均方根誤差為0.0930m/s,東北天速度精度分別提升了26.2%、29.3%和8.2%。由圖5可知,使用聯邦濾波算法時,經計算得到的緯度方向的均方根誤差為2.1193m,經度方向的均方根誤差為4.3058m,高度方向的均方根誤差為6.2032m;使用非等間隔信息處理算法后,經計算得到的緯度方向的均方根誤差為1.3842m,經度方向的均方根誤差為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

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

4 跑車實驗

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

圖6 跑車實驗路線圖Fig.6 Roadmap of vehicle experiment

本次跑車實驗先給導航板上電正常啟動,等待至接收機定位且進入組合導航階段,輸出穩定后啟動車輛進行跑車測試。本次測試選用NovAtel-KVH1750作為參考系統,測試得到的數據與其進行速度、位置對比,結果如圖7、圖8所示。

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

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

實驗過程中,部分方向上初始誤差不同是因為截取了各傳感器均正常工作時的一段數據。由圖7可知,使用聯邦濾波算法時,經計算得到的東向速度的均方根誤差為0.1461m/s,北向速度的均方根誤差為0.2355m/s,天向速度的均方根誤差為0.3399m/s;使用非等間隔信息處理算法后,經計算得到的東向速度的均方根誤差為0.1007m/s,北向速度的均方根誤差為0.1544m/s,天向速度的均方根誤差為0.1201m/s,東北天速度精度分別提升了31.1%、34.4%和64.6%。由圖8可知,使用聯邦濾波算法時,經計算得到的緯度方向的均方根誤差為3.5225m,經度方向的均方根誤差為2.5570m,高度方向的均方根誤差為2.1985m;使用非等間隔信息處理算法后,經計算得到的緯度方向的均方根誤差為2.3899m,經度方向的均方根誤差為1.5371m,高度方向的均方根誤差為1.2726m,東北天位置精度分別提升了32.2%、39.9%和42.1%。

由跑車實驗測試結果可知,加入非等間隔濾波算法后,位置精度和速度精度都有較大提升,證明了異質異步非等間隔信息處理算法切實有效,應用于組合導航算法后能有效提高導航精度,具有較高的工程應用價值。

5 結論

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