1. 引言
由于卫星导航自身存在诸多限制,未来的导航系统向着多源导航方向发展,通过对多种传感器的导航数据进行信息融合,提升系统整体导航精度以及场景适应性。这种导航系统量测信息来源于各个导航传感器,不同传感器的适用范围不同,当载体所在场景不适用于某种传感器,或传感器自身器件出现故障,都会输出错误的导航信息,进而影响到系统的性能。
导致传感器出现故障的原因很多,很难进行归纳总结,但不同故障源引起的传感器输出故障信息具有一般性,主要可以分为软故障和硬故障两类,对于不同的故障类型,有着不同的检测方式。目前多传感器导航系统比较成熟的故障检测方法主要是基于卡尔曼滤波器的c2检验法,包括残差c2检验法、状态c2检验法、双状态c2检验法 [1] 。
残差c2检验法对于突变的硬故障具有较好的检测效果,但是不易检测出慢变故障,存在较大的延时。序贯概率比检验是残差判决中一种较为常用的方法 [2] ,相比于残差c2检验法,该方法更适合软故障检测 [3] 。JARMANKD等人 [4] 采用序贯概率比法对组合导航系统进行故障检测,刘昆朋 [2] 针对正态分布的检测量,提出了一种改进的序贯概率比故障检测算法,采用迭代递推的方法来计算故障检验函数,对于慢变的斜坡故障具有较高的灵敏度。但也正因为采用迭代递推法,当故障消失后,由于其对缓变故障的跟踪特性 [5] ,检验值需要经过一段时间恢复到阈值以下,从而无法准确判断故障的结束时间。
本文首先建立INS/GNSS/视觉组合导航系统,并对系统在无故障状态下的定位能力进行了仿真验证;之后基于残差c2检验法以及改进SPRT检测法的特性,研究了残差c2-SPRT联合故障检测算法,该方法与单一检测方法相比,对系统软故障及硬故障均有较好的响应,在系统出现故障时,能够较快的检测出发生故障的系统,为之后的故障隔离打下基础,同时能够判断故障结束时间,完成误警率和漏警率均为0.01时软故障及硬故障的检测。最后基于MATLAB软件,对这种检测算法进行仿真验证,并得出相应结论,即该算法可以很大程度上削弱故障对定位结果的影响,得到与系统正常工作是相近的定位结果,提升系统可靠性。
2. INS/GNSS/视觉组合导航系统设计
系统采用主滤波器状态方程无信息分配,有重置联邦滤波结构完成架构设计,其结构图如图1所示 [6] ,图中局部滤波器即为子滤波器。
![](//html.hanspub.org/file/6-2570187x9_hanspub.png)
Figure 1. Structure diagram of federated filter
图1. 联邦滤波器结构图
2.1. 系统状态模型
本文以GPS作为系统方案中GNSS部分,以SINS作为系统方案中INS部分,后文中的INS/GNSS均为SINS/GPS组合。在图1的结构中,参考系统选取SINS系统,子系统1为GPS系统,子系统2为视觉导航系统,视觉导航系统基于张洋 [7] 的双目视觉导航算法。
选择载体在导航坐标系下的导航信息误差及惯性器件漂移误差作为系统状态向量:
(1)
式中,
表示三个惯性导航平台误差角,
表示三个速度误差,
表示三个位置误差,
表示陀螺三个常值漂移误差和三个相关漂移误差,
表示加速度计三个相关漂移误差。
系统的状态方程为:
(2)
式中,F(t)表示组合系统状态方程的一步状态转移矩阵,由SINS系统误差模型得到,此处不过多赘述;G(t)表示组合系统状态方程的系统白噪声误差矩阵;W(t)为组合系统状态方程的系统误差白噪声矢量。
2.2. 系统量测模型
由图1结构,系统量测模型由SINS-GPS子系统量测模型及SINS-视觉子系统量测模型两部分组成。其中,SINS-GPS子系统为松耦合结构,其量测方程中的量测信息由SINS输出的三维速度、三维位置分别与GPS输出的三维速度、三维位置的差值构成:
(3)
式中,
为局部滤波器1的系统观测量,
为量测矩阵,
表示GPS在三维速度、三维位置上的白噪声误差。
SINS-视觉子系统量测方程中的量测信息由SINS输出的三维姿态、三维速度、三维位置分别与视觉导航系统输出的三维姿态、三维速度、三维位置的差值构成,三维姿态用,
来表示,分别为载体的俯仰角,偏航角及横滚角:
(4)
为姿态、速度、位置测量误差白噪声矢量。
3. 残差c2-SPRT联合故障检测与隔离算法
故障检测与隔离模块位于联邦滤波结构子滤波器之后,主滤波器之前,其输入为子滤波器残差,输出为故障检测结果,根据故障检测结果,决定子滤波器是否参与主滤波器信息融合。当判定某子滤波器故障时,断开其与主滤波器间的数据输入,以正常子滤波重新构筑子滤波部分,完成故障模块的隔离与系统重构,模块结构如图2所示。
3.1. 传感器故障模式
不同故障源引起的传感器输出故障信息具有一般性,主要可以分为软故障和硬故障两类。
软故障泛指传感器特性的变异,其特点为幅值较小,变化缓慢,具体包含数据的偏差、漂移、测量精度下降等等。在工程上,通常将软故障描述为一种随时间变化的故障信息,其故障值与时间变化成正相关,一般来说,将故障值和时间表述为线性关系或二次方关系,相应的故障信息表达式为:
(5)
式中,y表示故障值大小,k为故障系数,t0为故障开始时间,t为系统运行时间。
由表达式及示意图可知,软故障在故障开始发生时幅值较小,后续增加的快慢与故障系数k相关,当k值较小时,故障幅值会长时间保持小量,造成故障难以被检测,加大了故障检测的难度,因此,检测方法灵敏度的高低是衡量软故障检测算法好坏的一项重要标准。
硬故障泛指传感器结构损坏导致的故障,其特点为幅值较大,变化突然。硬故障也称完全故障,故障发生时,测量值不随实际变化而变化,始终保持某一读数,一般表现为较大的阶跃值,相应的故障信息表达式为:
(6)
式中,y的含义与式(5)相同,constant为常数。
由于硬故障幅值较大,且为突变故障,对系统影响较大,需要被快速检测并进行隔离,因此,检测方法的快速性是衡量硬故障检测算法的一项重要标准。
![](//html.hanspub.org/file/6-2570187x26_hanspub.png)
Figure 2. Fault detection and isolation module based on federated filter structure
图2. 基于联邦滤波结构的故障检测与隔离模块
3.2. 残差c2算法
组合导航系统中,对于每个子滤波器,基于卡尔曼滤波的残差值为:
(7)
式中,
为一步状态估计预报值
(8)
由于卡尔曼滤波为线性最小方差估计,在系统正常工作时,残差
为均值为零的高斯白噪声,此时方差为
(9)
当系统发生故障时,预报值相对真实量测存在偏差,残差
均值不再为零,因此可以利用残差
检测结果是否为零来判断系统是否发生故障,定义故障检测函数为
(10)
函数服从自由度为m的c2分布,即
,m为观测矩阵
的维数,故障判断准则为
(11)
式中,
表示设定的故障门限值,可由自由度m和误警率
查表获得,门限值的确定如表1所示。
当检测结果未超过设定门限值时,判定子滤波器正常工作,将其滤波结果送至主滤波器参与信息融合;当检测结果超过设定门限值时,判定子滤波器输出信息存在故障,对该子滤波器进行隔离,阻止其滤波结果参与信息融合,并利用剩余子系统重新构造联邦滤波结构,保证整体系统的数据可靠性。当检测到故障子滤波器输出恢复正常后,再重新将其输出信息加入主滤波器信息融合。
3.3. 改进SPRT故障检测算法
设未知正态随机变量x的k次序贯独立样本为
,由概率论与数学统计原理,近似有
,式中,样本均值和样本方差分别为
(12)
(13)
设状态变量x的实际量测值为x*,无故障时,有
;发生故障时,
,其中,x0表示无故障时的真实值。
定义正常类H0:
;故障类H1:
,则观测序列一定属于这两个样本之一,且其概率密度函数为
(14)
(15)
其中,
,进而可得其似然比为
(16)
对式(16)两侧取自然对数,可得对数似然比为
(17)
综上所述,似然比
的递推计算公式为
(18)
检验阈值
由误警率
和漏警率
计算得到,由Wald序贯概率算法 [8] 可得
(19)
故障判决准则为
(20)
3.4. 联合检测算法
基于残差c2检验法以及改进SPRT检测法对不同类型故障的敏感特性,本文采用了一种残差c2-SPRT联合故障检测算法,保证故障检测的快速准确,其结构如图3所示。
设检测发生故障时,检测结果输出为0;检测未发生故障时,检测结果输出为1,则根据检测结果1、2,故障判决器判决规则如下:
1、残差c2检测输出1,SPRT检测输出1。两种检测算法均为检测出故障,判定系统工作正常,检测
![](//html.hanspub.org/file/6-2570187x61_hanspub.png)
Figure 3. Structure diagram of joint fault detection algorithm
图3. 联合故障检测算法结构图
结果3输出为1;
2、残差c2检测输出0,SPRT检测输出0。两种检测算法均检测出故障,判定系统出现故障,检测结果3输出为0;
3、残差c2检测输出0,SPRT检测输出1。由于改进SPRT检测较残差c2检测,在故障出现时更为灵敏,且在故障消失时似然比需要一段时间恢复到阈值以下,因此这种情况几乎不会出现,理论上不计入讨论范围,实际出现时,按系统故障处理,检测结果3输出为0;
4、残差c2检测输出1,SPRT检测输出0。若为故障发生阶段,改进SPRT检测先于残差c2检测发现系统小量故障,该情况下,故障导致改进SPRT检测似然比增大,即当
增量为正时,判定系统出现故障,检测结果3输出为0;若为故障消失阶段,改进SPRT检测似然比向阈值以下减小,即当
增量为负时,判定系统工作正常,检测结果3输出为1。
4. 仿真与分析
4.1. 仿真系统搭建
系统仿真平台如图4所示,平台主要包括载体运动轨迹生成模块、GPS信号仿真模块、惯性测量器件仿真模块、双目视觉导航仿真模块、故障检测与隔离模块、信息融合滤波模块以及误差分析模块。
仿真轨迹选取无人机飞行轨迹,飞行时长202秒,飞行高度在80~90米之间,初始位置为东经110˚,北纬40˚,轨迹为无人机在空中的转弯前行,飞行轨迹如图5所示。陀螺仪常值漂移0.2˚/h,加速度计零偏0.001 g,白噪声均方差0.001 g,陀螺及加速度计一阶马尔科夫相关时间3600 s;GPS位置测量误差(x, y, z) = (5 m, 5 m, 10 m),速度测量误差0.1 m/s;摄像机焦距6.56 mm,像素物理尺寸7.4 × 10−3 mm,立体视觉测量基线4 m,摄像机分辨率1024 × 1024;故障检测与隔离模块中,设定误警率
,漏警率
,SINS/GPS子系统观测量为6维,
,SINS/视觉子系统观测量为9维,
,
;仿真时间更新频率为1 Hz;基于MATLAB软件进行仿真。
为验证故障检测与隔离算法的有效性,人为设置故障情况,在40-80s内对GPS高度观测量加入0.5 m/s的线性故障,进行软故障检测;在120~160 s的时间段内对GPS高度观测量加入50 m阶跃误差,进行硬故障检测。经过仿真验证,在其他观测量上人为添加故障后检测结果类似,由于篇幅原因,本文仅以GPS高度故障检测进行分析,其余观测量故障检测结果不再赘述。
4.2. 仿真结果
在上述条件下进行仿真,未进行人为添加故障时的系统定位位置误差如图6所示。人为添加故障后,
![](//html.hanspub.org/file/6-2570187x71_hanspub.png)
Figure 6. Position error without fault
图6. 无故障时位置误差
故障检测模块中两种检测方式在高度方向的检测值及最终的故障状态判定如图7所示;未进行故障检测和进行本文故障检测算法处理后的位置误差结果分别如图8,图9所示。
由图6可知,无故障时,导航位置误差在经度和纬度方向上均在4 m以内,高度方向上在1 m以内,即所设计的INS/GNSS/视觉组合导航系统能够满足定位需求。
由图7可知,在40 s时间点添加缓变故障后,由于残差c2检测对软故障不敏感,其检测值在50 s后超过检测阈值,存在10 s以上的检测延迟,而SPRT高度检测值超过阈值的时间延迟较短,仅为5 s左右,较残差c2检测有很大提高;在80 s时间点故障消失时,残差c2检测值很快回到阈值以下,能够很好反映
![](//html.hanspub.org/file/6-2570187x72_hanspub.png)
Figure 7. Fault detection value and fault determination result
图7. 故障检测值及故障判定结果
![](//html.hanspub.org/file/6-2570187x73_hanspub.png)
Figure 8. Position error without fault detection and isolation
图8. 未进行故障检测与隔离定位位置误差
![](//html.hanspub.org/file/6-2570187x74_hanspub.png)
Figure 9. Position error after fault detection and isolation
图9. 经过故障检测与隔离后定位位置误差
故障结束时间,而SPRT高度检测值回归速度较慢。在80 s时间点添加阶跃故障时,残差c2检测值立即上升至阈值以上,能够快速检测阶跃故障,而SPRT高度检测值由于未能快速回归至阈值以下,不能通过数值大小判定系统是否故障,但故障发生后,SPRT检测值呈上升趋势,在联合算法中,可依据SPRT检测值变化趋势判定系统是否故障;故障消失时情况同缓变故障。由联合检测算法故障判定结果可知,该算法对于软故障及硬故障,均有较好的检测效果,在载体运动过程中同时发生软故障及硬故障时,联合检测算法均能够较为快速的进行故障检测,并根据3.3节判定准则给出准确的判定结果。
对比图8和图9可知,未进行故障检测隔离时,故障信息进入主滤波器并污染整个系统,造成高度定位值误差增大,在40~80 s软故障发生时,位置误差由正常时的1 m以内增加到20 m以上,在120~160 s硬故障发生时,位置误差进一步增大,且故障消失后,由于对系统的污染,高度定位误差存在米级震荡,不满足定位精度需求。进行故障检测与隔离后,高度误差在1 m以内,与正常状态相近,满足定位精度需求,且在其他两个方向上,定位误差无显著增大,可认为联合检测算法对软故障及硬故障均能够较为准确的进行检测,并通过对故障模块的隔离,保证系统整体定位精度仍满足需求。
5. 结束语
本文基于联邦滤波结构,设计了一种INS/GNSS/视觉组合导航系统,其定位精度能够满足定位需求,并在此基础上,将残差c2检测算法与改进SPRT检测法结合,提出了一种联合检测算法。仿真结果表明,该联合检测算法能够及时检测出故障发生,并对故障部分进行隔离;且能及时判断故障消失时间,减少误警现象发生。由于联邦滤波结构的灵活性,可添加更多的传感器,分别于参考系统构成新的子滤波器,基于这种结构的检测算法适用于多种传感器组成的多源导航系统,提高系统对于故障的处理能力,保证系统的定位可靠性。