基于抗差自适应滤波的多源融合导航算法

2022-11-03谭聚豪杨傲爽李天宇马训穷

航天控制 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。