基于抗差自適應濾波的多源融合導航算法

2022-11-03 03:45:42譚聚豪楊傲爽李天宇馬訓窮

航天控制 2022年5期

譚聚豪楊傲爽李天宇馬訓窮陳帥

1. 上海航天電子技術研究所，上海 201109 2. 南京理工大學，南京 210094

0 引言

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

1 多源信息融合模型導航方案設計

1.1 導航信息源

(1)捷聯慣性導航系統

本文中使用捷聯慣性導航系統模型作為信息融合模型的基準框架，使之與其他導航傳感器進行信息融合，SINS的輸出信息為：

(1)

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

(2)BDS衛星導航系統

BDS衛星接收機對接收到的衛星信號進行處理，其解算出的位置、速度信息如下式所示：

(2)

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

(3)氣壓高度計

氣壓高度計通過利用內部傳感器測量的海拔高度的表達式為：

halt=hr-δhalt

(3)

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

(4)里程計

(4)

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

(5)

(5)電子羅盤

電子羅盤可以對載體的航向角進行測量，則與航向角真值的關系如下：

ψIM=ψ-δψIM

(6)

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

1.2 多源導航模型

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

(7)

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

SINS/BDS的狀態方程為：

(8)

SINS/BDS的量測方程為：

(9)

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

(10)

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

(11)

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

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

SINS/里程計狀態方程為：

(12)

SINS/里程計量測方程為：

(13)

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

(14)

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

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

SINS/氣壓高度計狀態方程為：

(15)

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

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

(16)

式中,

(17)

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

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

SINS/電子羅盤狀態方程為：

(18)

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

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

(19)

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

(20)

2 抗差自適應卡爾曼濾波算法設計

為了控制動力學模型和觀測信息的擾動影響，本文采用整體控制狀態噪聲協方差陣的方法，如果觀測信息出現異常，就對觀測信息進行抗差估計處理；如果動力學模型存在異常，就采用自適應因子調整動力學模型信息，這就是抗差自適應濾波處理。

k時刻新息向量的表達式為：

(21)

k時刻新息向量的協方差表達式為：

(22)

(23)

則量測信息的協方差陣為：

(24)

為了控制狀態模型異常和觀測信息異常對系統狀態參數估計的影響，利用抗差估計原理構造如下極值函數：

(25)

(26)

(27)

將信息向量方程和狀態預測信息的誤差方程帶入上式，可得狀態參數的抗差自適應濾波解為：

(28)

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

(29)

(30)

抗差自適應濾波就是把標準卡爾曼的濾波計算放在計算回路里，具體如下：

(31)

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

(32)

則可以求得抗差自適應濾波遞推解為：

(33)

也可以得到相應的狀態參數協方差陣為：

(34)

為了避免αk為0時，抗差濾波解會出現奇異的情況，可取兩段函數表示自適應因子，具體為：

(35)

式中,c取1.0～2.5。

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

圖1 抗差自適應濾波算法流程圖

3 濾波發散判斷與處理

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

卡爾曼濾波的收斂判據可以依據狀態誤差的協方差陣P的對角線元素進行判斷，如果在經過一次濾波后其相應狀態誤差協方差陣P中對應元素項小于設定的閾值，則認為濾波收斂。

在本系統中，姿態誤差協方差Pδφ的閾值設置為1，速度誤差協方差Pδv的閾值設置為0.04，位置誤差協方差PδP的閾值設置為25，只對這幾項進行判斷即可判斷濾波器收斂與否，具體判斷如下：

(36)

而對濾波器發散的判斷，我們可以基于濾波器的新息進行，在前面小節中我們知道濾波器的新息序列rk和其新息方差Crk的表達式為：

(37)

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

設置如下的判斷檢測函數[11]：

(38)

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

4 仿真實驗與結果分析

4.1 仿真流程設計

本節將通過半實物仿真實驗對多源融合模型下的導航方案以及濾波方法進行驗證。下面給出具體仿真實驗設計流程圖，見圖2。

圖2 仿真實驗流程

4.2 仿真參數設置

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