CN115358319A - 基于双阈值检测的自适应容错滤波方法及系统 - Google Patents

Abstract

本发明公开一种基于双阈值检测的自适应容错滤波方法和系统，包括：根据六自由度位姿测量系统的量测与状态信息建立组合滤波系统的状态空间方程；提取滤波新息向量构建残差卡方检测量，设定双阈值进行系统状态判定；当判定系统故障时，构建D‑RBFNN网络，将典型训练集附加实时训练样本输入D‑RBFNN网络并使用改进K‑均值聚类与奇异值分解进行训练获取对系统姿态误差进行估计，获取姿态误差估计值；同时用LSSVR的故障检测算法进行复合检测，获取系统的状态误差估计值；当判定系统正常时，将量测信息直接输入状态空间方程进行自适应容错KF递推获取系统的状态误差估计值；根据获取的状态误差估计值对当前系统状态进行补偿获取当前系统位姿信息。

Description

基于双阈值检测的自适应容错滤波方法及系统

技术领域

本发明涉及滤波数据处理技术领域，具体涉及一种基于双阈值检测的自适应容错滤波方法及系统。

背景技术

随着航空航天、船舶制造、盾构工程等大空间、协同式智能制造工程的发展，对六自由度位姿测量的高精度、实时性、高鲁棒等要求不断提高，传统的单传感器定位技术并不能满足实际需要。多传感器融合技术及一些新兴的组合系统滤波融合算法成为研究热点。激光标靶测量系统(林嘉睿,邾继贵,郭寅,任永杰,杨凌辉,高凯.一种高精度六自由度测量系统及方法[P].天津：CN107246866A,2017-10-13.)是基于倾角传感的六自由度测量系统，能够在室内等信号闭塞的工业环境，如车间、隧道、地下工程中提供稳定高精度的位置与姿态，但在一些含动态测量需求的工业场景，如轨道检测、盾构等工程中，其测角性能会急剧下降。捷联惯导系统(SINS)是一种自主导航系统，抗干扰能力强、稳定性高，能不依赖于外部传感器提供完整的位姿测量值，但受工作原理限制其测量误差随时间产生累积，以至长时间工作后测量数据发散。因此，使用单个传感器不能保证精确稳定的六自由度位姿的测量，为此，多传感器融合算法研究有着重要价值。

在传感器融合领域，建立融合精度高，容错能力强的滤波估计算法对于提升系统测量精度与鲁棒性至关重要。经典的卡尔曼滤波算法(KF)以其收敛速度快，融合精度高的特点在传感器融合领域获得广泛应用，然而其滤波精度依赖于系统模型与噪声协方差的准确性，因此，需根据系统传感器工作状态结合环境噪声因素对系统结构及方差进行自适应调整。此外，为了提升组合系统测量稳定性，需建立完整的故障检测、隔离与补偿(FDIR)系统。

目前，常用的故障检测算法对于阶跃式的故障形式具有较好的检测性能，然而对于故障随时间缓慢变化的渐变式“软”故障，会发生滤波状态跟踪故障，导致故障野值污染滤波器与状态估计精度降低。此外，当系统检测出故障子系统后，通常将子系统的测量数据直接进行隔离，即不将该子系统的测量信息输入组合系统滤波算法之中，由于缺少了有效的量测信息并缺少误差的补偿方式，必然会给整个系统的精度与性能带来影响。

发明内容

本发明的目的在于提供一种基于双阈值检测的自适应容错滤波方法及系统。所述方法首先设定双阈值进行故障检测，并在判断在激光标靶子系统倾角测量失准情况下，将典型训练集附加实时训练样本输入D-RBFNN网络获取系统误差的状态估计值；同时在判断为检测量在双阈值之间时，构建基于LSSVR的故障检测和残差卡方检测的复合故障检测算法以保证系统对于不同形式的故障失效的检测性能，当系统处于正常状态时，直接调用信息融合模块实现组合系统的数据融合，从而保证六自由度位姿测量精度，提升系统的容错能力与测量鲁棒性。

本发明的目的是通过以下技术方案实现的：

一种基于双阈值检测的自适应容错滤波方法，应用于六自由度位姿测量系统，所述六自由度位姿测量系统包括激光标靶和捷联惯导系统，包括如下过程：

步骤一：根据六自由度位姿测量系统的量测与状态信息建立组合滤波系统的状态空间方程；其中量测与状态信息包括激光跟踪仪和倾角仪获取的量测信息和捷联惯导系统获取的姿态、速度与位置信息；

步骤二：提取滤波新息向量构建残差卡方检测量，设定双阈值判断检测量从而进行系统状态判定；

步骤三：当检测量残差大于等于双阈值的高阈值时，系统处于故障状态，执行步骤3-1，即构建D-RBFNN网络，采集六自由度位姿测量系统在匀速状态下绕三轴旋转样本作为典型训练集，采集六自由度位姿测量系统在检测工作正常状态下前N个时刻的样本作为实时训练样本，N作为实时训练样本个数；将典型训练集附加实时训练样本输入D-RBFNN网络并使用改进K-均值聚类与奇异值分解进行训练获取对系统姿态误差进行估计，获取姿态误差估计值；随后将获取的姿态误差估计值输入状态空间方程进行自适应容错卡尔曼滤波递推，获取系统的状态误差估计值；

当检测量残差位于双阈值之间时，执行步骤3-2，即利用LSSVR的故障检测算法获取新息预测值从而构建新的故障检测函数FDF_k进行复合故障检测；当大于等于设定的故障检测阈值时，执行步骤3-1，否则，构建新息方差统计量求取方差调节因子并输入调节函数模型中对量测方差自适应调整，获取调整后的量测信息；之后将获取的量测信息输入状态空间方程进行自适应容错KF递推，获取系统的状态误差估计值；其中，LSSVR的故障检测算法将滤波新息向量作为训练输出，核函数采用RBF核，中心宽度采用正则化原则选取；

当检测量残差小于双阈值的低阈值时，

将步骤一的量测信息直接输入滤波系统状态空间方程进行自适应容错卡尔曼滤波递推，获取系统的状态误差估计值；

步骤四：根据获取的系统的状态误差估计值对当前系统状态进行补偿获取当前系统位姿信息。

进一步的，步骤3-1中D-RBFNN网络包括输入层、隐含层、输出层，其中输入层选用对姿态误差影响较大的水平方向速度、滚转角、俯仰角和方位角作为输入；输出层为对应于故障状态下预测的姿态误差；且隐含节点输出为输入层的高斯高维映射，输出层节点输出对应为隐含层节点的线性权连接；

其中，D-RBFNN网络的隐含层的中心选取采用改进K-均值取类与奇异值分解实现在线训练与预测，具体包括：

1)固定N₀个典型训练样本进行经典K-均值聚类，确定初始聚类数k₀与隐函数中心向量j＝1,2,...k₀。

2)将样本数N_t实时采样样本X_i加入训练集之中，计算对应样本的适应度函数f_ij：

并提取与输入样本适应度最大的聚类中心C_j；

3)对X_i与C_j的适应度f_ij与适应度阈值f₀做如下判定：

若f_ij≤f₀，则X_i归于C_j所在聚类之中，并按如下原则更新聚类中心：

若f_ij＞f₀，则增加新的聚类中心，且设置对应X_i为对应第k+1个聚类的中心C_k+1。

进一步的，倾角仪获取滚转角与俯仰角，经过CCD成像靶面获得的入射光图像坐标经双矢量定姿模型求解方位角，并且激光跟踪仪接收激光标靶的返回光获取水平方向速度。

进一步的，步骤3-2中基于LSSVR的故障检测算法包括如下计算步骤：

将系统当前k时刻前T个时刻量测信息Z_i作为训练输入，对应滤波新息向量r_i作为训练输出，核函数采用RBF核，中心宽度采用正则化原则选取；构造如下回归函数：

f(x)＝ω^Tφ(x)+b

其中，ω为权向量，b为偏差量，φ(·):Rⁿ→R^m为将样本空间映射到高维特征空间的核空间映射函数；

根据给定的样本训练集Ω＝{(x_i,y_i)|i＝1,2,...,N}，转换LSSVR回归函数为拉格郎日函数，随后通过求偏导和最小二乘解进行拟合，获得LSSVR拟合回归函数：

将当前时刻量测信息Z_k作为预测输入，获取对应在理想工作状态下的预测新息

定义变量

其中r_k为滤波新息；

从而确定基于LSSVR的故障检测函数FDF_k为

其中，δ_k是定义的变量，Δ_k是δ_k对应协方差矩阵，

其中A_k为滤波新息协方差阵；

当倾角仪发生故障时，变量δ_k均值不再为零，从而引起故障检测函数FDF_k增大，设置故障检测阈值为T_d，由此构建如下故障检测规则：

当FDF_k≥T_d时，认为系统处于故障状态，执行步骤3-1；

当FDF_k<T_d时，认为系统处于正常状态，构建新息方差统计量求取方差调节因子并输入调节函数模型中对量测方差自适应调整，获取调整后的量测信息；之后将获取的量测信息输入状态空间方程进行自适应容错KF递推，获取系统的状态误差估计值。

进一步的，所述步骤3-2中的“构建新息方差统计量求取方差调节因子并输入调节函数模型中对量测方差自适应调整”的具体步骤如下：

以滤波新息的协方差特性作为判定系统受故障影响程度的判定指标，定义方差调节因子β_k衡量新息的实际统计方差与理论方差的偏离程度，

其中，D_k为新息实际统计方差，A_k为新息理论方差，D_k通过滑窗法计算其新息方差的无偏估计统计量获取，

M为对应窗长度，

为k时刻对应时间窗内新息的均值，A_k可通过滤波方程计算，

在获取方差调节因子基础上，构建方差调节函数Etanh(x)模型，方差调节函数为加强的双曲正切函数，保持原有双曲正切函数在其频区延展特性的基础上，在高频区增加了斜率从而保证故障增加时系统可以进一步隔离失准信息，增强系统容错能力，Etanh方差调节函数定义如下，

Etanh(0)＝0,Etanh(1)＝1

将方差调节因子β_k传入方差调节函数中，按如下规则对量测方差进行自适应调整

R_k＝Etanh(β_k)R_k-1

由此实现系统受故障影响下量测方差自适应调整。

进一步的，步骤三中“进行自适应容错卡尔曼滤波递推,获取系统的状态误差估计值”具体包括：

系统的状态误差估计值滤波递推式具体如下：

X_k+1/k＝Φ_k+1/kX_k/k+Γ_k+1/ku_k

R_k＝Etanh(β_k)R_k-1

r_k+1＝Z_k+1-H_k+1X_k+1/k

X_k+1/k+1＝X_k+1/k+K_k+1r_k+1

其中，L_k+1为渐消矩阵，X_k/k为k时刻的状态向量，P_k/k为k时刻的状态协方差阵，Φ_k+1/k为状态转移矩阵，Γ_k+1/k为控制矩阵，u_k为k时刻的控制变量，Q_k+1为控制量协方差阵，H_k+1为量测矩阵，R_k为量测方差阵，Z_k+1为量测向量，r_k+1为新息向量，K_k+1为卡尔曼增益；

其中，当检测量残差小于双阈值的低阈值时，方差调节因子β_k＝1，并将步骤一的量测信息直接输入滤波系统状态空间方程；

当检测量残差位于双阈值之间时，方差调节因子β_k按照步骤3-2所述的量测方差自适应调整方法进行自适应调节。

当检测量残差大于等于高阈值时，方差调节因子β_k＝1，并将步骤3-1获取的姿态误差值导入滤波系统状态空间方程。

本发明还公开了一种基于双阈值检测的自适应容错滤波系统，包括存储器、处理器、六自由度位姿测量系统、数据输入端口，所述数据输入端口用于输入来自六自由度位姿测量系统量测的滚转角与俯仰角、方位角和水平方向速度信息；所述存储器用于存储计算机程序，所述处理器用于处理数据，所述六自由度位姿测量系统包括作为基站的激光跟踪仪和6D激光标靶，所述6D激光标靶固定在待测目标上，包括倾角仪和捷联惯导系统；所述处理器处理的步骤包括：通过滤波新息向量构建残差卡方检测量，设定双阈值进行故障检测；当检测量残差小于双阈值的低阈值时，将六自由度位姿测量系统的量测信息直接输入滤波系统状态空间方程进行自适应容错卡尔曼滤波递推，获取系统的状态误差估计值；当检测量残差大于等于双阈值的高阈值时，构建D-RBFNN网络，将典型训练集附加实时训练样本输入D-RBFNN网络并使用改进K-均值聚类与奇异值分解进行训练对系统姿态误差进行估计，获取姿态误差估计值；随后将获取的姿态误差估计值输入状态空间方程进行自适应容错KF递推，获取系统的状态误差估计值；当检测量残差位于双阈值之间时，将检测量输入LSSVR故障检测模块获取新息预测值从而构建新的故障检测函数FDF_k进行复合故障检测，当大于故障检测阈值时，按检测量残差大于等于双阈值的高阈值时的步骤进行处理，否则，构建新息方差统计量求取方差调节因子并输入调节函数模型中对量测方差自适应调整，随后再将修正后的量测信息输入滤波系统状态空间方程进行自适应容错卡尔曼滤波递推，获取系统的状态误差估计值；其中，LSSVR故障检测模块将滤波新息向量作为训练输出，核函数采用RBF核，中心宽度采用正则化原则选取；最后根据获取的系统的状态误差估计值对当前系统状态进行补偿获取当前系统位姿信息。

与现有技术相比，本发明的技术方案所带来的有益效果是：

所述方法首先设定双阈值进行故障检测，在判断激光标靶子系统倾角测量失准情况下通过构建D-RBFNN网络进行误差补偿；在判断为检测量在双阈值之间时，构建基于LSSVR的故障检测和残差卡方检测的复合故障检测算法，并引入方差调节因子对量测方差阵进行自适应调节；当系统处于正常状态时，直接调用信息融合模块实现组合系统的数据融合，从而保证六自由度位姿测量精度，提升系统的容错能力与测量鲁棒性；

步骤3-1中构建基于D-RBFNN的失准姿态误差预测与补偿算法，在系统故障状态下，通过D-RBFNN网络对失准姿态进行回归预测，该网络能根据实时训练样本自适应调整结构参数，从而提升网络泛化能力，提升姿态误差预测精度，保证了在故障状态下姿态测量的精度与可靠性；

步骤3-2中构建一种基于卡方故障检测与基于LSSVR的复合故障检测算法，通过卡方检测对阶跃式故障进行检测并采用基于LSSVR的故障检测算法提升对软故障的检测能力，通过LSSVR对滤波新息进行回归并结合系统滤波新息构建故障检测量，从而降低系统噪声与模型偏置对故障检测影响，提升检测灵敏度，采用复合故障检测算法提升系统对于不同形式的故障失效的检测能力；

同时步骤3-2中还构建一种量测方差自适应调整算法，通过计算方差调节因子衡量系统受故障影响程度，并构建调节函数从而在子系统产生失准时对量测方差进行自适应调整，从而保证故障增加时系统可以进一步隔离失准信息，增强系统容错能力，从而根据系统受故障影响程度调节量测信息在状态估计中影响权重，增强了系统的容错能力，提升了状态估计精度。

附图说明

图1是本发明所述的基于双阈值检测的自适应容错滤波方法流程图；

图2是激光标靶/捷联惯导组合系统结构示意图；

图3是RBFNN网络结构示意图；

图4是本发明所述的自适应容错滤波方法中步骤三中的改进K-均值聚类算法流程图。

其中，

201：激光跟踪仪，202：激光标靶，203：角锥棱镜，204：CCD成像靶面，205：双轴倾角仪，206：SINS

具体实施方式

为使本发明实施例的目的、技术方案、有益效果及显著进步更加清楚，下面结合本发明实施例中所提供的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所有描述的这些实施例仅是本发明的部分实施例，而不是全部的实施例；基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，一种基于双阈值检测的自适应容错滤波方法，应用于如图2所示的六自由度位姿测量系统，所述测量系统包括激光跟踪仪201和6D激光标靶202，所述激光跟踪仪201作为基站，所述6D激光标靶202固定在待测目标上，包括角锥棱镜203、CCD成像靶面204、双轴倾角仪205、捷联惯导系统(SINS)206和数据处理单元，所述SINS包括陀螺仪和加速度计。所述激光跟踪仪201接收角锥棱镜203返回光获取位置测量信息，CCD成像靶面204接收角锥棱镜透射光构成小孔映射模型并结合双轴倾角仪205获取的二轴姿态信息，通过矢量定姿模型可得到激光靶的姿态值。所述CCD成像靶面204、双轴倾角仪205和SINS206分别与数据处理单元电连接，且所述数据处理单元接收来自所述CCD成像靶面204、双轴倾角仪205和SINS206的数据并传送至上位机。

所述上位机构建自适应容错滤波算法实现激光标靶与捷联惯导系统(SINS)的融合，具体包括如图1所示的基于双阈值检测的自适应容错滤波方法，包括如下步骤：

步骤1：通过激光标靶202获取待测目标的姿态值φ_P，其中所述姿态值φ_P包括倾角仪205、CCD成像靶面204获取的姿态信息；通过激光跟踪仪201接收激光标靶返回测量光获取待测目标位置p_P，并通过高频采点差分运算获取待测目标速度v_P；再通过SINS206获取待测目标姿态

速度v_S与位置p_S，由此确定所述测量系统在k时刻量测向量

根据系统可被观测的状态量与SINS中的误差分量，确定系统状态向量

φ_x,φ_y,φ_z分别表示SINS系统的滚转角误差、俯仰角误差与方位角误差；δv_x，δv_y，δv_z分别表示SINS在X轴、Y轴、Z轴上的速度误差；δp_x，δp_y，δp_z分别表示SINS在X轴、Y轴、Z轴上的位置误差；ε_gb，ε_gr分别表示陀螺仪的静态偏差与动态偏差，ε_ab，ε_ar分别表示加速度计的静态偏差与动态偏差。由此构建如下组合滤波系统状态空间方程：

其中，X_k/k为k时刻的状态向量，P_k/k为k时刻的状态协方差阵，Φ_k+1/k为状态转移矩阵，Γ_k+1/k为控制矩阵，u_k为k时刻的控制变量，Q_k+1为控制量协方差阵，H_k+1为量测矩阵，R_k为量测方差阵，Z_k+1为量测向量。

根据对滤波方程进行容错结构设计，可在激光标靶产生动态失准时根据系统工作状态自适应调整量测方程以及时隔离故障信息保证状态估计精度，增强组合系统的容错能力。

步骤2：提取滤波新息向量构建残差卡方检测量，设定双阈值判断检测量从而进行系统状态判定；具体包括如下步骤：

定义新息向量r_k为k时刻量测向量与系统量测预测值之差，表示为：

r_k＝Z_k-H_kX_k/k-1

根据新息理论，当系统无故障时，新息向量r_k+1符合均值为零的高斯白噪声分布，其方差为：

当系统受故障影响时，新息向量r_k+1的均值不再是零，并且随故障的幅值而变化，因此通过对新息向量r_k+1的均值检验来判断系统受故障影响的程度，可构建如下故障检测量：

根据新息序列的统计特性，故障检测量λ_d符合自由度为m的卡方分布，即λ_d～χ²(m)，为了进一步减小故障检测的漏检率与误触率，增强组合系统的容错能力提升状态估计精度，采用双阈值复合故障检测的组合滤波算法，即设置高阈值T_H与低阈值T_L并构建如下故障检测规则：

当λ_d≥T_H时，系统处于故障状态，执行步骤3-1；

当T_H＞λ_d≥T_L时，执行步骤3-2，对系统状态进行复合故障检测并对量测方差自适应调整；

当λ_d＜T_L时，系统处于正常状态，将步骤一的量测信息直接输入状态空间方程进行自适应容错KF递推，获取系统的状态误差估计值。

由于倾角仪存在于激光标靶内部，主要用于获取姿态角中的俯仰角与滚转角，但是倾角仪传感会产生动态失效，进而导致激光标靶测角失效，本专利将倾角仪正常的状态即视为六自由度位姿测量系统工作正常，因为倾角仪的故障会引起所述六自由度激光标靶测量系统的故障。因此，当检测倾角仪工作正常状态时，定义系统的量测矩阵H_k+1：

其中，I为3×3单位阵。

当检测倾角仪处于故障状态时，定义系统的量测矩阵H_k+1：

步骤3-1：当步骤二获得的故障检测量λ_d大于等于高阈值，构建动态-径向基神经网络(D-RBFNN)对失准姿态误差进行预测，采集六自由度位姿测量系统在匀速状态下绕三轴旋转样本作为典型训练集，采集六自由度位姿测量系统在检测工作正常状态下前N个时刻的样本作为实时训练样本，N作为实时训练样本个数；将典型训练集附加实时训练样本输入D-RBFNN误差预测模型对姿态误差进行估计；

如图3所示，RBFNN由输入层、隐含层、输出层组成，其中隐含节点输出为输入层的高斯高维映射，输出层节点输出对应为隐含层节点的线性权连接。首先确定D-RBFNN输入层与输出层节点，输入层选用对姿态误差影响较大的水平方向速度(v_x，v_y)与滚转角(γ)，俯仰角(θ)和方位角(α)作为输入；其中倾角仪获取滚转角γ与俯仰角θ，经过CCD成像靶面204获得的入射光图像坐标经双矢量定姿模型求解方位角α，并且激光跟踪仪201接收激光标靶202的返回光获取速度v_x，v_y；输出层为对应于故障状态下预测的姿态误差(δγ，δθ，δα)。每个隐含节点与输入层之间采用径向基函数作为激活函数，第j个隐含层节点对应输出O_j为：

其中X_i为对应第i个输入向量样本，C_j为隐含层对应第j个高斯单元的中心向量，σ_j为隐含层中心宽度，其表征输入向量对该中心的敏感程度，N为训练样本数，k为隐含层节点数目。每个输出节点与隐含层采用线性权连接，第m个输出节点值y_m为：

其中ω_mj为第m个输出节点与第j个隐含层节点之间的连接权值，M为输出节点数目。

隐含层的中心选取对于网络的逼近能力影响较大，目前常用的是k均值聚类方法确定隐节点数据与中心位置，通过聚类过程在已知训练输入样本的情况下能够较准确获取聚类数与对应聚类中心。然而由于其聚类过程耗时过大只适用于固定样本，考虑在工程应用中为了增强网络泛化能力并根据工作环境与工作模式实时对网络结构进行自适应在线调整，所述D-RBFNN神经网络引入改进K-均值聚类与奇异值分解(SVD)实现在线训练与预测，能够在固定样本基础上不断加入实时样本以增强网络抗环境干扰能力，增强网络的回归精度，如图4所示，一种改进k均值聚类算法过程如下：

1)固定N₀个典型训练样本进行经典K-均值聚类，确定初始聚类数k₀与隐函数中心向量j＝1,2,...k₀。

2)将样本数N_t实时采样样本X_i加入训练集之中，计算对应样本的适应度函数f_ij：

并提取与输入样本适应度最大的聚类中心C_j。

3)对X_i与C_j的适应度f_ij与适应度阈值f₀做如下判定：

若f_ij≤f₀，则X_i归于C_j所在聚类之中，并按如下原则更新聚类中心：

若f_ij＞f₀，则增加新的聚类中心，且设置对应X_i为对应第k+1个聚类的中心C_k+1。

由此完成改进K-均值聚类过程，在此基础上，采用SVD算法快速求取隐含层与输出层连接权值，对应于第m个输出节点y_m输出为：

则在整个样本空间中对应的输出矩阵为

连接权值矩阵为W_M×k，隐含层输出矩阵为

可通过以下SVD求解W_M×k，

其中，U和V分别是隐含层输出矩阵Ω的左右奇异矩阵，隐节点基函数中心宽度采用正则化原则选取，由此完成了D-RBFNN网络的构建，该网络在系统处于故障阶段时生效，并将实时样本加入训练集之中增强网络回归精度，从而保证在子系统失准时，组合系统姿态测量精度稳定。

步骤3-2：当检测值位于高低阈值之间，基于LSSVR的故障检测算法进行复合故障检测，具体步骤如下：

滤波新息向量能够反映系统的故障水平，故将其作为故障检测统计量，然而其受系统噪声滤波模型偏置的影响会导致对故障检测灵敏度降低，由此构建基于LSSVR的故障检测算法，将系统当前k时刻前T个时刻量测信息Z_i作为训练输入，对应滤波新息向量r_i作为训练输出，核函数采用RBF核，中心宽度采用正则化原则选取。构造如下回归函数：

f(x)＝ω^Tφ(x)+b

其中，ω为权向量，b为偏差量，φ(·):Rⁿ→R^m为将样本空间映射到高维特征空间的核空间映射函数。对于给定的样本训练集Ω＝{(x_i,y_i)|i＝1,2,...,N}，LSSVR回归函数可转化为求解如下含等式约束的优化问题：

s.t.y_i＝ω^Tφ(x_i)+b+ζ，i＝1,2,...,N

其中，C为正则化参数，ζ为误差量，上述优化问题可转换为求解如下Lagrange函数：

其中，α_i≥0为Lagrange乘子。利用KT条件通过上述Lagrange函数分别对ω，b，ξ，α求偏导，可得：

由此，可消去变量ω与ζ，上述方程可描述为如下形式：

其中，K_ij＝K(x_i,x_j)＝φ(x_i)^Tφ(x_j)，在本方法中采用高斯RBF核表示，由此可以获取如下最小二乘解:

其中A⁺表示A的广义逆，从而确定LSSVR回归函数：

在LSSVR完成训练后可获取拟合回归函数，将当前时刻量测信息Z_k作为预测输入，可获取对应在理想工作状态下的预测新息

定义变量

其中r_k为滤波新息，回归值符合训练集样本的协方差特征，且认为滤波新息与回归相互独立，可计算δ_k对应协方差矩阵：

其中A_k为滤波新息协方差阵，当系统无故障时，变量δ_k符合零均值高斯白噪声分布；当系统发生故障，则δ_k的均值不再为零。据此，可对变量δ_k做出如下二元假设：

H₁:E(δ_k)＝μ,E((δ_k-μ)(δ_k-μ)^T)＝Δ_k

对应条件概率密度函数如下：

计算P_r(δ|H₁)与P_r(δ|H₀)的对数似然比为，

当δ_k＝μ时，对数似然比L_k取极大值，由此确定基于LSSVR的故障检测函数FDF_k为

当六自由度位姿测量系统发生故障时，变量δ_k均值不再为零，从而引起故障检测函数FDF_k增大，设置故障检测阈值为T_d(根据系统工作状态与环境因素人为设定)，由此构建如下故障检测规则：

当FDF_k≥T_d时，系统处于故障状态，执行步骤3-1；

当FDF_k＜T_d时，系统处于正常状态，构建新息方差统计量求取方差调节因子并输入调节函数模型中对量测方差自适应调整。

需要指出的是，基于LSSVR的故障检测算法对于渐变故障及赋值较小的“软故障”具有检测高灵敏度与准确性，然而对于较大的故障形式会出现LSSVR难以高精度逼近从而检测性能下降的问题，因此，基于LSSVR的故障检测算法用作结合残差卡方检测的复合故障检测算法以保证系统对于不同形式的故障失效的检测性能。

具体的，步骤3-2中的“量测方差自适应调整”具体包括如下步骤：

以滤波新息的协方差特性作为判定系统受故障影响程度的判定指标，定义方差调节因子β_k衡量新息的实际统计方差与理论方差的偏离程度，

其中，D_k为新息实际统计方差，A_k为新息理论方差，D_k通过滑窗法计算其新息方差的无偏估计统计量获取，

M为对应窗长度，

为k时刻对应时间窗内新息的均值，A_k可通过滤波方程计算，

在获取方差调节因子基础上，构建方差调节函数Etanh(x)模型，方差调节函数为加强的双曲正切函数，保持原有双曲正切函数在其频区延展特性的基础上，在高频区增加了斜率从而保证故障增加时系统可以进一步隔离失准信息，增强系统容错能力，Etanh方差调节函数定义如下，

Etanh(0)＝0,Etanh(1)＝1

将方差调节因子β_k传入方差调节函数中，按如下规则对量测方差进行自适应调整

R_k＝Etanh(β_k)R_k-1

由此实现系统受故障影响下量测方差自适应调整，从而根据系统受故障影响程度调节量测信息在状态估计中影响权重，增强了系统的容错能力，提升了状态估计精度。

步骤3-3：当检测值小于低阈值，将步骤一的六自由度测量系统获得的量测信息直接输入状态空间方程，进行自适应容错卡尔曼递推，获取系统的状态误差估计值；

系统的状态误差估计值滤波递推式具体如下：

X_k+1/k＝Φ_k+1/kX_k/k+Γ_k+1/ku_k

R_k＝Etanh(β_k)R_k-1

r_k+1＝Z_k+1-H_k+1X_k+1/k

X_k+1/k+1＝X_k+1/k+K_k+1r_k+1

其中，L_k+1为渐消矩阵，X_k/k为k时刻的状态向量，P_k/k为k时刻的状态协方差阵，Φ_k+1/k为状态转移矩阵，Γ_k+1/k为控制矩阵，u_k为k时刻的控制变量，Q_k+1为控制量协方差阵，H_k+1为量测矩阵，R_k为量测方差阵，Z_k+1为量测向量，r_k+1为新息向量，K_k+1为卡尔曼增益；

其中，当检测量残差小于双阈值的低阈值时，方差调节因子β_k＝1，并将步骤一的量测信息直接输入滤波系统状态空间方程；

当检测量残差位于双阈值之间时，方差调节因子β_k按照步骤3-2所述的量测方差自适应调整方法进行自适应调节。

当检测量残差大于等于高阈值时，方差调节因子β_k＝1，并将步骤3-1获取的姿态误差值导入滤波系统状态空间方程。

步骤四：根据获取的系统状态量误差估计值对当前系统状态进行补偿获取当前系统位姿信息。

以上实施例仅用以说明本发明的技术方案，而非是对其的限制，尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换，而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围，本领域技术人员根据本说明书内容所做出的非本质改进和调整或者替换，均属本发明所要求保护的范围。

Claims (7)

1.一种基于双阈值检测的自适应容错滤波方法，其特征在于，所述滤波系统应用于六自由度位姿测量系统，所述六自由度位姿测量系统包括激光标靶和捷联惯导系统，包括如下步骤：

步骤一：根据六自由度位姿测量系统的量测与状态信息建立组合滤波系统的状态空间方程；其中量测与状态信息包括激光跟踪仪和倾角仪获取的量测信息和捷联惯导系统获取的姿态、速度与位置信息；

步骤二：提取滤波新息向量构建残差卡方检测量，设定双阈值判断检测量从而进行系统状态判定；

步骤三：当检测量残差大于等于双阈值的高阈值时，系统处于故障状态，执行步骤3-1，即构建D-RBFNN网络，采集六自由度位姿测量系统在匀速状态下绕三轴旋转样本作为典型训练集，采集六自由度位姿测量系统在检测工作正常状态下前N个时刻的样本作为实时训练样本，N作为实时训练样本个数；将典型训练集附加实时训练样本输入D-RBFNN网络并使用改进K-均值聚类与奇异值分解进行训练获取对系统姿态误差进行估计，获取姿态误差估计值；随后将获取的姿态误差估计值输入状态空间方程进行自适应容错卡尔曼滤波递推，获取系统的状态误差估计值；

当检测量残差位于双阈值之间时，执行步骤3-2，即利用LSSVR的故障检测算法获取新息预测值从而构建新的故障检测函数FDF_k进行复合故障检测；当大于等于设定的故障检测阈值时，执行步骤3-1，否则，构建新息方差统计量求取方差调节因子并输入调节函数模型中对量测方差自适应调整，获取调整后的量测信息；之后将获取的量测信息输入状态空间方程进行自适应容错KF递推，获取系统的状态误差估计值；其中，LSSVR的故障检测算法将滤波新息向量作为训练输出，核函数采用RBF核，中心宽度采用正则化原则选取；当检测量残差小于双阈值的低阈值时，将步骤一的量测信息直接输入滤波系统状态空间方程进行自适应容错卡尔曼滤波递推，获取系统的状态误差估计值；

步骤四：根据获取的系统的状态误差估计值对当前系统状态进行补偿获取当前系统位姿信息。

2.根据权利要求1所述的基于双阈值检测的自适应容错滤波方法，其特征在于，步骤3-1中D-RBFNN网络包括输入层、隐含层、输出层，其中输入层选用对姿态误差影响较大的水平方向速度、滚转角、俯仰角和方位角作为输入；输出层为对应于故障状态下预测的姿态误差；且隐含节点输出为输入层的高斯高维映射，输出层节点输出对应为隐含层节点的线性权连接；

其中，D-RBFNN网络的隐含层的中心选取采用改进K-均值取类与奇异值分解实现在线训练与预测，具体包括：

1)固定N₀个典型训练样本进行经典K-均值聚类，确定初始聚类数k₀与隐函数中心向量j＝1,2,...k₀；

2)将样本数N_t实时采样样本X_i加入训练集之中，计算对应样本的适应度函数f_ij：

并提取与输入样本适应度最大的聚类中心C_j；

3)对X_i与C_j的适应度f_ij与适应度阈值f₀做如下判定：

若f_ij≤f₀，则X_i归于C_j所在聚类之中，并按如下原则更新聚类中心：

若f_ij＞f₀，则增加新的聚类中心，且设置对应X_i为对应第k+1个聚类的中心C_k+1。

3.根据权利要求1所述的基于双阈值检测的自适应容错滤波方法，其特征在于，六自由度位姿测量系统中的倾角仪获取滚转角与俯仰角，经过CCD成像靶面(204)获得的入射光图像坐标经双矢量定姿模型求解方位角，并且激光跟踪仪(201)接收激光标靶(202)的返回光获取水平方向速度。

4.根据权利要求1所述的基于双阈值检测的自适应容错滤波方法，其特征在于，步骤3-2中基于LSSVR的故障检测算法包括如下计算步骤：

将系统当前k时刻前T个时刻量测信息Z_i作为训练输入，对应滤波新息向量r_i作为训练输出，核函数采用RBF核，中心宽度采用正则化原则选取；构造如下回归函数：

f(x)＝ω^Tφ(x)+b

其中，ω为权向量，b为偏差量，φ(·):Rⁿ→R^m为将样本空间映射到高维特征空间的核空间映射函数；

根据给定的样本训练集Ω＝{(x_i,y_i)|i＝1,2,...,N}，转换LSSVR回归函数为拉格郎日函数，随后通过求偏导和最小二乘解进行拟合，获得LSSVR拟合回归函数：

将当前时刻量测信息Z_k作为预测输入，获取对应在理想工作状态下的预测新息

定义变量

其中r_k为滤波新息；

从而确定基于LSSVR的故障检测函数FDF_k为

其中，δ_k是定义的变量，Δ_k是δ_k对应协方差矩阵，

其中A_k为滤波新息协方差阵；

当倾角仪发生故障时，变量δ_k均值不再为零，从而引起故障检测函数FDF_k增大，设置故障检测阈值为T_d，由此构建如下故障检测规则：

当FDF_k≥T_d时，认为系统处于故障状态，执行步骤3-1；

当FDF_k<T_d时，认为系统处于正常状态，构建新息方差统计量求取方差调节因子并输入调节函数模型中对量测方差自适应调整，获取调整后的量测信息；之后将获取的量测信息输入状态空间方程进行自适应容错KF递推，获取系统的状态误差估计值。

5.根据权利要求4所述的基于双阈值检测的自适应容错滤波方法，其特征在于，所述步骤3-2中的“构建新息方差统计量求取方差调节因子并输入调节函数模型中对量测方差自适应调整”的具体步骤如下：

以滤波新息的协方差特性作为判定系统受故障影响程度的判定指标，定义方差调节因子β_k衡量新息的实际统计方差与理论方差的偏离程度，

其中，D_k为新息实际统计方差，A_k为新息理论方差，D_k通过滑窗法计算其新息方差的无偏估计统计量获取，

M为对应窗长度，

为k时刻对应时间窗内新息的均值，A_k可通过滤波方程计算，

在获取方差调节因子基础上，构建方差调节函数Etanh(x)模型，方差调节函数为加强的双曲正切函数，保持原有双曲正切函数在其频区延展特性的基础上，在高频区增加了斜率从而保证故障增加时系统可以进一步隔离失准信息，增强系统容错能力，Etanh方差调节函数定义如下，

Etanh(0)＝0,Etanh(1)＝1

将方差调节因子β_k传入方差调节函数中，按如下规则对量测方差进行自适应调整

R_k＝Etanh(β_k)R_k-1

由此实现系统受故障影响下量测方差自适应调整。

6.根据权利要求1所述的基于双阈值检测的自适应容错滤波方法，其特征在于，步骤三中“进行自适应容错卡尔曼滤波递推,获取系统的状态误差估计值”具体包括：

系统的状态误差估计值滤波递推式具体如下：

X_k+1/k＝Φ_k+1/kX_k/k+Γ_k+1/ku_k

R_k＝Etanh(β_k)R_k-1

r_k+1＝Z_k+1-H_k+1X_k+1/k

X_k+1/k+1＝X_k+1/k+K_k+1r_k+1

其中，L_k+1为渐消矩阵，X_k/k为k时刻的状态向量，P_k/k为k时刻的状态协方差阵，Φ_k+1/k为状态转移矩阵，Γ_k+1/k为控制矩阵，u_k为k时刻的控制变量，Q_k+1为控制量协方差阵，H_k+1为量测矩阵，R_k为量测方差阵，Z_k+1为量测向量，r_k+1为新息向量，K_k+1为卡尔曼增益；

其中，当检测量残差小于双阈值的低阈值时，方差调节因子β_k＝1，并将步骤一的量测信息直接输入滤波系统状态空间方程；

当检测量残差位于双阈值之间时，方差调节因子β_k按照步骤3-2所述的量测方差自适应调整方法进行自适应调节。

当检测量残差大于等于高阈值时，方差调节因子β_k＝1，并将步骤3-1获取的姿态误差值导入滤波系统状态空间方程。

7.一种基于双阈值检测的自适应容错滤波系统，其特征在于，包括存储器、处理器、六自由度位姿测量系统、数据输入端口，所述数据输入端口用于输入来自六自由度位姿测量系统量测的滚转角与俯仰角、方位角和水平方向速度信息；所述存储器用于存储计算机程序，所述处理器用于处理数据，所述六自由度位姿测量系统包括作为基站的激光跟踪仪和6D激光标靶，所述6D激光标靶固定在待测目标上，包括倾角仪和捷联惯导系统；所述处理器处理的步骤包括：通过滤波新息向量构建残差卡方检测量，设定双阈值进行故障检测；当检测量残差小于双阈值的低阈值时，将六自由度位姿测量系统的量测信息直接输入滤波系统状态空间方程进行自适应容错卡尔曼滤波递推，获取系统的状态误差估计值；当检测量残差大于等于双阈值的高阈值时，构建D-RBFNN网络，将典型训练集附加实时训练样本输入D-RBFNN网络并使用改进K-均值聚类与奇异值分解进行训练对系统姿态误差进行估计，获取姿态误差估计值；随后将获取的姿态误差估计值输入状态空间方程进行自适应容错KF递推，获取系统的状态误差估计值；当检测量残差位于双阈值之间时，将检测量输入LSSVR故障检测模块获取新息预测值从而构建新的故障检测函数FDF_k进行复合故障检测，当大于故障检测阈值时，按检测量残差大于等于双阈值的高阈值时的步骤进行处理，否则，构建新息方差统计量求取方差调节因子并输入调节函数模型中对量测方差自适应调整，随后再将修正后的量测信息输入滤波系统状态空间方程进行自适应容错卡尔曼滤波递推，获取系统的状态误差估计值；其中，LSSVR故障检测模块将滤波新息向量作为训练输出，核函数采用RBF核，中心宽度采用正则化原则选取；最后根据获取的系统的状态误差估计值对当前系统状态进行补偿获取当前系统位姿信息。

Images