基于抗差自適應(yīng)濾波的多源融合導(dǎo)航算法

譚聚豪 楊傲爽 李天宇 馬訓(xùn)窮 陳 帥

1. 上海航天電子技術(shù)研究所，上海 201109 2. 南京理工大學(xué)，南京 210094

0 引言

多傳感器融合定位可以整合多傳感器的信息，且信息源豐富，導(dǎo)航信息可靠、穩(wěn)定。目前“SINS+多傳感器”的組合導(dǎo)航系統(tǒng)的研究和應(yīng)用日益廣泛，尤其以“SINS+GNSS”的架構(gòu)居多。當(dāng)衛(wèi)星導(dǎo)航系統(tǒng)正常工作時，采用“SINS+GNSS”的組合模型可以實時修正SINS系統(tǒng)的誤差，進(jìn)而實現(xiàn)可靠性導(dǎo)航；但是當(dāng)衛(wèi)星信號被遮擋或者觀測信息受到較大擾動時，其導(dǎo)航的精度則不再保持可靠，因此研究異常情況下的誤差抑制方法以及無衛(wèi)星情況下的融合方案極其重要。國內(nèi)的一些機(jī)構(gòu)、學(xué)者也對GPS、氣壓高度計、原子鐘、里程計、磁強(qiáng)計、車輛航位推算定位(Dead Reckoning, DR)、INS等兩兩組合或者三者組合的相關(guān)問題進(jìn)行了研究[1]。西北工業(yè)大學(xué)研究的基于激光捷聯(lián)慣導(dǎo)系統(tǒng)、里程計和GPS的組合導(dǎo)航系統(tǒng)，在運動學(xué)約束的模型下，水平定位精度可以達(dá)到行駛路程的千分之一[2]；北京理工大學(xué)采用SINS/ODO組合，利用卡爾曼濾波器對捷聯(lián)慣性導(dǎo)航系統(tǒng)和里程計進(jìn)行校正，可在線估計里程計刻度因子，同時還可以對故障進(jìn)行判斷，有效提高了系統(tǒng)的導(dǎo)航精度[3]。北京航空航天大學(xué)采用SINS/ODO/BDS三者組合，采用聯(lián)邦卡爾曼濾波器，仿真測試BDS在失效3600s、緯度誤差50m、經(jīng)度誤差15m時，系統(tǒng)仍能保持較高精度[4]。哈爾濱工業(yè)大學(xué)對偏振光/地磁/GPS/SINS的組合模型進(jìn)行了研究，結(jié)果證明在同時使用偏振光和地磁的情況下，其測姿性能具有長期穩(wěn)定性的特點[5]。有學(xué)者利用因子圖對超短基線(USBL)/多普勒測速儀(DVL)/SINS的三者導(dǎo)航系統(tǒng)進(jìn)行了建模，研究水下潛航器的定位問題[6]。也有學(xué)者提出基于增量平滑因子圖的SINS/BD/DVL/CNS船用導(dǎo)航系統(tǒng)信息融合算法，并以量測信息的更新來進(jìn)行融合架構(gòu)的構(gòu)建[7]。還有學(xué)者對無人機(jī)著陸中的多源信息，采用內(nèi)插外推法與基于Savitzky-Golay濾波的信號平滑方法，分別實現(xiàn)時間配準(zhǔn)與野點平滑，并利用基于自適應(yīng)窗長的動態(tài)方差估計融合算法,對多傳感器信息進(jìn)行融合[8]。以上諸多學(xué)者、機(jī)構(gòu)對導(dǎo)航源進(jìn)行了一定數(shù)目的選擇性融合，并構(gòu)建了導(dǎo)航模型和濾波算法。但隨著導(dǎo)航傳感器數(shù)目的增加，以及多源導(dǎo)航信息異步、異質(zhì)情況的出現(xiàn)，其算法的應(yīng)用場景以及濾波方式的局限性逐步顯現(xiàn)，因此必須兼顧并解決多源信息融合過程中的動力學(xué)模型異常以及量測信息異常的影響，以此來提高多源導(dǎo)航系統(tǒng)的魯棒性。而本文構(gòu)建了SINS/BDS/里程計/氣壓高度計/電子羅盤的融合導(dǎo)航模型，研究了復(fù)雜環(huán)境下多源融合導(dǎo)航可能出現(xiàn)動力學(xué)模型異常以及量測信息異常情況的抗差自適應(yīng)導(dǎo)航濾波算法，此外本文設(shè)計的濾波算法可實時調(diào)節(jié)動力學(xué)模型和量測信息，并通過半實物仿真實驗進(jìn)行了多源融合模型的驗證，以及異常情況下濾波算法精度的驗證。

1 多源信息融合模型導(dǎo)航方案設(shè)計

1.1 導(dǎo)航信息源

(1)捷聯(lián)慣性導(dǎo)航系統(tǒng)

本文中使用捷聯(lián)慣性導(dǎo)航系統(tǒng)模型作為信息融合模型的基準(zhǔn)框架，使之與其他導(dǎo)航傳感器進(jìn)行信息融合，SINS的輸出信息為：

(1)

式中,Lr，λr，hr，VE，VN和VU為位置的真值和速度的真值，δL，δλ，δh，δVINS_E，δVBDS_N和δVINS_U為慣性導(dǎo)航系統(tǒng)的定位誤差和測速誤差。

(2)BDS衛(wèi)星導(dǎo)航系統(tǒng)

BDS衛(wèi)星接收機(jī)對接收到的衛(wèi)星信號進(jìn)行處理，其解算出的位置、速度信息如下式所示：

(2)

式中,δLBDS,δλBDS,δhBDS,δVBDS_E,δVBDS_N和δVBDS_U為北斗接收機(jī)的定位誤差和測速誤差。

(3)氣壓高度計

氣壓高度計通過利用內(nèi)部傳感器測量的海拔高度的表達(dá)式為：

halt=hr-δhalt

(3)

式中,δhalt為高度計定位誤差。

(4)里程計

(4)

將里程計輸出的載體前向速度分解到導(dǎo)航坐標(biāo)系，可以表示成式(5)所示：

(5)

(5)電子羅盤

電子羅盤可以對載體的航向角進(jìn)行測量，則與航向角真值的關(guān)系如下：

ψIM=ψ-δψIM

(6)

式中,ψ為航向角的真值，δψIM為電子羅盤的測量誤差。

1.2 多源導(dǎo)航模型

選取SINS的誤差量作為公共狀態(tài)量，并加入陀螺的常值誤差、隨機(jī)誤差以及加速度計的隨機(jī)誤差，這樣可以得到18維的狀態(tài)量XI，具體形式如下[9]：

(7)

(1)SINS/BDS組合導(dǎo)航模型

SINS/BDS的狀態(tài)方程為：

(8)

SINS/BDS的量測方程為：

(9)

其中,HI和VI具體形式如下所示：

(10)

VI=[δLBDSδλBDSδhBDSδVBDS_EδVBDS_NδVBDS_U]

(11)

量測噪聲VI包括位置量測噪聲和速度量測噪聲，把兩者均看作是均值為0的白噪聲處理。

(2) SINS/里程計組合導(dǎo)航模型

SINS/里程計狀態(tài)方程為：

(12)

SINS/里程計量測方程為：

(13)

其中，HODO為觀測矩陣，VODO為量測噪聲，具體如下：

(14)

式中,量測噪聲VODO作為白噪聲處理。

(3) SINS/氣壓高度計組合導(dǎo)航模型

SINS/氣壓高度計狀態(tài)方程為：

(15)

SINS/氣壓高度計量測方程為：

ZALT=[hINS-halt]=[δh-δhalt]=
HALT(t)XI(t)+VALT(t)

(16)

式中,

(17)

量測噪聲VALT作為白噪聲處理。

(4) SINS/電子羅盤組合導(dǎo)航模型

SINS/電子羅盤狀態(tài)方程為：

(18)

SINS/電子羅盤量測方程為：

ZIM(t)=HIM(t)XIM(t)+VIM(t)

(19)

式中，VIM是電子羅盤測量航向角的測量白噪聲。

(20)

2 抗差自適應(yīng)卡爾曼濾波算法設(shè)計

為了控制動力學(xué)模型和觀測信息的擾動影響，本文采用整體控制狀態(tài)噪聲協(xié)方差陣的方法，如果觀測信息出現(xiàn)異常，就對觀測信息進(jìn)行抗差估計處理；如果動力學(xué)模型存在異常，就采用自適應(yīng)因子調(diào)整動力學(xué)模型信息，這就是抗差自適應(yīng)濾波處理。

k時刻新息向量的表達(dá)式為：

(21)

k時刻新息向量的協(xié)方差表達(dá)式為：

(22)

(23)

則量測信息的協(xié)方差陣為：

(24)

為了控制狀態(tài)模型異常和觀測信息異常對系統(tǒng)狀態(tài)參數(shù)估計的影響，利用抗差估計原理構(gòu)造如下極值函數(shù)：

(25)

(26)

(27)

將信息向量方程和狀態(tài)預(yù)測信息的誤差方程帶入上式，可得狀態(tài)參數(shù)的抗差自適應(yīng)濾波解為：

(28)

由于預(yù)測殘差信息可以較好地反應(yīng)擾動影響，因此我們可以采用預(yù)測殘差構(gòu)造自適應(yīng)因子αk[10]，采用三段函數(shù)法可以表示如下：

(29)

(30)

抗差自適應(yīng)濾波就是把標(biāo)準(zhǔn)卡爾曼的濾波計算放在計算回路里，具體如下：

(31)

利用自適應(yīng)因子αk可以使濾波的穩(wěn)定性得到一定改善，但不能有效抵制觀測信息異常的影響，因此，在濾波中增加抗差性，要求動力學(xué)模型中的協(xié)方差信息匹配當(dāng)前觀測信息，實時調(diào)節(jié)增益矩陣Kk，具體如下：

(32)

則可以求得抗差自適應(yīng)濾波遞推解為：

(33)

也可以得到相應(yīng)的狀態(tài)參數(shù)協(xié)方差陣為：

(34)

為了避免αk為0時，抗差濾波解會出現(xiàn)奇異的情況，可取兩段函數(shù)表示自適應(yīng)因子，具體為：

(35)

式中,c取1.0～2.5。

綜上可以得到抗差自適應(yīng)濾波算法的流程如圖1所示：

圖1 抗差自適應(yīng)濾波算法流程圖

3 濾波發(fā)散判斷與處理

在“SINS+多傳感器”的數(shù)據(jù)融合濾波過程中，濾波是一個平穩(wěn)、收斂的過程，這樣才能不斷地修正SINS的誤差，進(jìn)而抑制誤差的發(fā)散，在濾波平穩(wěn)收斂之后才對濾波過程的發(fā)散進(jìn)行判斷。

卡爾曼濾波的收斂判據(jù)可以依據(jù)狀態(tài)誤差的協(xié)方差陣P的對角線元素進(jìn)行判斷，如果在經(jīng)過一次濾波后其相應(yīng)狀態(tài)誤差協(xié)方差陣P中對應(yīng)元素項小于設(shè)定的閾值，則認(rèn)為濾波收斂。

在本系統(tǒng)中，姿態(tài)誤差協(xié)方差Pδφ的閾值設(shè)置為1，速度誤差協(xié)方差Pδv的閾值設(shè)置為0.04，位置誤差協(xié)方差PδP的閾值設(shè)置為25，只對這幾項進(jìn)行判斷即可判斷濾波器收斂與否，具體判斷如下：

(36)

而對濾波器發(fā)散的判斷，我們可以基于濾波器的新息進(jìn)行，在前面小節(jié)中我們知道濾波器的新息序列rk和其新息方差Crk的表達(dá)式為：

(37)

在濾波平穩(wěn)階段，新息序列rk是零均值的白噪聲；當(dāng)濾波不平穩(wěn)，也即濾波發(fā)散時，新息的均值就不再為0了，因此對殘差的檢驗可以確定系統(tǒng)濾波是否發(fā)散。

設(shè)置如下的判斷檢測函數(shù)[11]：

(38)

式中，λ是服從自由度為m的χ2分布，可以根據(jù)χ2分布表、信息的置信度以及實際濾波計算情況設(shè)置濾波發(fā)散的門限值。這樣既可保證判斷的靈敏度，又可防止出現(xiàn)誤判現(xiàn)象，有效地保證了濾波的可靠性和系統(tǒng)的安全。

4 仿真實驗與結(jié)果分析

4.1 仿真流程設(shè)計

本節(jié)將通過半實物仿真實驗對多源融合模型下的導(dǎo)航方案以及濾波方法進(jìn)行驗證。下面給出具體仿真實驗設(shè)計流程圖，見圖2。

圖2 仿真實驗流程

4.2 仿真參數(shù)設(shè)置

SINS/BDS/里程計(ODO) /電子羅盤(Compass) /氣壓高速計(Alt)融合導(dǎo)航仿真實驗需要根據(jù)器件的特性設(shè)置參數(shù)，由于進(jìn)行的是半實物仿真實驗，因此采集的是真實傳感器數(shù)據(jù)，通過采集真實車載環(huán)境下的傳感器數(shù)據(jù)，再根據(jù)仿真要求添加異常噪聲，其中SINS系統(tǒng)、BDS接收機(jī)、里程計(ODO)、氣壓高度計(Alt)以及電子羅盤(Compass)的關(guān)鍵仿真參數(shù)如見表1～5。

表1 慣性測量器件參數(shù)

表2 BDS接收機(jī)的仿真參數(shù)

表3 里程計仿真參數(shù)

表4 氣壓高度計仿真參數(shù)

表5 電子羅盤仿真參數(shù)

4.3 仿真實驗結(jié)果

本節(jié)仿真參數(shù)的設(shè)置根據(jù)各個傳感器的性能參數(shù)以及(4.2)小節(jié)中的方法進(jìn)行設(shè)置，接著在數(shù)據(jù)仿真過程的80s～130s時間段和240s～290s時間段內(nèi)分別加入均值為0、方差為30的白噪聲和均值為0、方差為20的白噪聲，分析濾波算法的誤差抑制能力以及濾波導(dǎo)航精度。

圖3～5為3種濾波算法下的速度誤差對比結(jié)果；圖6～8為位置誤差對比結(jié)果；圖9～11為整個過程速度曲線對比結(jié)果；圖12～ 14為整個過程位置曲線對比結(jié)果。具體仿真結(jié)果如下：

圖3 東向速度誤差對比

圖4 北向速度誤差對比

圖5 天向速度誤差對比

圖6 緯度誤差對比

圖7 經(jīng)度誤差對比

圖8 高度誤差對比

圖9 東向速度對比

圖10 北向速度對比

圖11 天向速度對比

圖12 緯度對比結(jié)果

圖13 經(jīng)度對比結(jié)果

圖14 高度對比結(jié)果

5 實驗分析與結(jié)論

分析實驗中的對比曲線可以發(fā)現(xiàn)，抗差自適應(yīng)濾波的水平位置誤差在1.67m左右，高度誤差在6.6m左右，水平速度誤差在0.24m/s以內(nèi)，天向速度誤差在0.11m/s左右。而卡爾曼濾波和自適應(yīng)濾波的位置誤差和速度誤差較大。

受異常擾動后，常規(guī)的卡爾曼濾波和自適應(yīng)濾波的性能在天向速度變化和高度變化上均受到強(qiáng)烈影響，而抗差自適應(yīng)濾波則表現(xiàn)出較強(qiáng)的抑制噪聲擾動能力。

從整體性能上看，抗差自適應(yīng)濾波的性能優(yōu)于自適應(yīng)濾波及常規(guī)卡爾曼濾波，其穩(wěn)定性更好，濾波精度更高，可以動態(tài)調(diào)節(jié)系統(tǒng)噪聲矩陣信息，實時抑制擾動，消除動力學(xué)模型擾動和量測信息異常帶來的影響，進(jìn)而使濾波保持平穩(wěn)，輸出可靠的導(dǎo)航信息。