CN105785790B - 一种基于无人系统自控状态下滤波切换控制单元的控制方法 - Google Patents

Abstract

一种无人系统自控状态下滤波切换控制单元和方法，所述滤波切换控制单元包括标准卡尔曼滤波单元、抗干扰卡尔曼滤波单元以及切换控制单元，所述滤波切换控制方法包括首先根据无人系统所采用的滤波系统所受外部干扰特性，建立基于外部干扰的离散状态空间模型，然后将该离散状态空间模型与原系统模型联立得到状态增广系统，之后设计标准卡尔曼滤波器并估计其状态，最后，根据标准卡尔曼滤波单元输出和抗干扰卡尔曼滤波单元输出与对应阈值的比较结果，判断干扰作用大小，决定切换滤波模式，从而进行反馈控制。

Description

一种基于无人系统自控状态下滤波切换控制单元的控制方法

技术领域

本发明涉及一种无人系统自控状态下滤波切换控制单元和方法，通过切换控制单元实现滤波系统在标准卡尔曼滤波和抗干扰卡尔曼滤波两种工作模式之间进行切换。本发明可应用于小型无人机等航空器和航天器以及飞行器的姿态控制系统的导航与自动控制系统，用于实现高精度姿态与位置测量。

背景技术

对于卫星、导弹和无人系统等为了完成各自所承担的任务，需要精确地测量自身的姿态，卫星、导弹和无人系统中的运载体等运动体在自动控制状态中需要实时获取自身运动参数等导航信息。当前绝大多数导航系统均采用多传感器组合导航的策略以弥补单一传感器性能上的不足，因此组合导航系统的精度与传感器性能和组合导航算法性能密切相关，由于在传感器性能提升空间上受现实条件的制约条件较大，故提升组合导航算法的性能成为保证组合导航精度的重要手段。

众所周知，卡尔曼滤波是绝大多数组合导航算法的核心部分，在系统模型准确且仅受到高斯白噪声作用时，卡尔曼滤波能给出系统状态在估计误差方差意义下的最优估计值。然而，当系统受到未知的外部扰动时，滤波精度将严重下降甚至会发生滤波发散现象，因此，实时估计并补偿系统外部扰动的影响就变得至关重要。干扰观测器是近些年发展起来的一种干扰估计方法，基于干扰观测器的控制作为抗干扰控制方法的一种，已在多个应用领域中证明了其有效性。与基于干扰观测器的控制类似，基于干扰观测器的滤波方法也是一种典型的抗干扰滤波方法，其主要设计步骤包括：首先建立干扰子系统的动态模型，在此基础上将干扰模型与原系统模型联立得到状态增广系统，最后针对状态增广系统设计标准卡尔曼滤波器同时估计干扰子系统状态和原系统状态。然而，基于干扰观测器的滤波方法的引入使状态变量维数增加，从而增加了滤波器增益阵与协方差阵求取的复杂程度，这将大幅增加滤波器的计算量，导致计算误差的累计效应更加明显。因此，在外部干扰较小，对滤波系统的影响可以忽略不计时，仍然采用基于干扰观测器的滤波方法将大大增加计算负担，使滤波精度受到影响。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种用于自控无人系统的、结构简单的，集成方便的，既能够有效抑制外部干扰的影响，又可以最大限度地降低计算量的滤波切换控制单元和方法。

本发明的技术解决方案为：一种基于无人系统自控状态下滤波切换控制单元的控制方法，所述滤波切换控制单元包括标准卡尔曼滤波单元、抗干扰卡尔曼滤波单元以及切换控制单元，所述滤波切换控制方法包括以下步骤：

(1)根据无人系统所采用的滤波系统所受外部干扰特性，建立基于外部干扰的离散状态空间模型为：

其中w(k)表示k时刻干扰子系统的状态变量；δ(k)表示k时刻干扰子系统输入，为高斯零均值白噪声；d(k)为k时刻滤波系统所受的干扰；W，H，V为已知的系数矩阵。

(2)将步骤(1)建立的基于外部干扰的离散状态空间模型与原系统模型联立，得到状态增广系统为：

其中为k时刻的增广状态；y(k)表示k时刻状态增广系统输出量测值；ω(k)，υ(k)为高斯零均值不相关白噪声；F，G，M为已知的系数矩阵，I为单位阵。

(3)根据步骤(2)得到的状态增广系统，设计标准卡尔曼滤波器估计其状态；

针对状态增广系统的卡尔曼滤波器设计由如下递推形式给出：

其中为k时刻先验估计误差协方差阵；为k时刻估计误差协方差阵；为k-1时刻新息向量；K_k为k时刻卡尔曼滤波增益阵。得到的估计值之后，再由和就可以同时估计出外部干扰和原系统状态。

(4)根据标准卡尔曼滤波单元输出和抗干扰卡尔曼滤波单元输出与对应阈值的比较结果，判断干扰作用大小，决定切换滤波模式；

将标准卡尔曼滤波单元输出的新息向量的2-范数||Δ_k||₂和抗干扰卡尔曼滤波单元输出的干扰估计值的2-范数送入切换控制单元，按以下方式进行判断：

1)当时，说明系统所受干扰作用较小，切换控制单元输出的选通信号σ选通标准卡尔曼滤波单元，将滤波系统工作模式切换至标准卡尔曼滤波模式；

2)当时，说明系统所受干扰作用较大，切换控制单元输出的选通信号σ选通抗干扰卡尔曼滤波单元，将滤波系统工作模式切换至抗干扰卡尔曼滤波模式。

本发明的原理是：当无人系统用滤波系统工作在标准卡尔曼滤波模式下时，切换控制单元实时检测标准卡尔曼滤波单元输出的新息向量以及抗干扰卡尔曼滤波单元输出的干扰估计值，如果新息向量的2-范数与干扰估计值的2-范数之和大于所设定的阈值，则切换控制单元将作出原系统正受到较严重的外部扰动作用的判断，于是将无人系统用滤波系统由标准卡尔曼滤波模式切换至抗干扰卡尔曼滤波模式。在抗干扰卡尔曼滤波模式下，首先利用关于干扰的部分已知信息，如干扰的频率、变化率等，建立干扰动态过程的状态空间模型，即把干扰写成动态系统输出变量的形式，然后将原系统模型与干扰系统模型联立得到状态增广系统，针对状态增广系统采用标准卡尔曼滤波得到外部扰动和原系统状态的同时估计。如果新息向量的2-范数与干扰估计值的2-范数之和小于所设定的阈值时，认为干扰的影响比较微弱可以忽略不计，将无人机用滤波系统由抗干扰卡尔曼滤波模式切换至标准卡尔曼滤波模式。在标准卡尔曼滤波模式下，将仅针对原系统进行滤波而不再考虑干扰模型，直至切换控制单元再次检测到较严重的外部扰动作用。

需要说明的是，抗干扰卡尔曼滤波与标准卡尔曼滤波的主要区别在于抗干扰卡尔曼滤波增加了干扰建模的步骤，将滤波系统所受的外部干扰视作高斯白噪声作用下线性系统的输出变量，将干扰模型与原系统动态方程联立，采用标准卡尔曼滤波算法估计联立所得状态增广系统的状态，即可实现外部干扰与原系统状态的同时估计。

本发明与现有技术相比的优点在于：本发明采用了标准卡尔曼滤波与抗干扰卡尔曼滤波两种工作模式，由切换控制单元根据干扰影响的大小控制工作模式的切换，在外部干扰作用较小时采用标准卡尔曼滤波以减小计算量，降低计算误差；在外部干扰作用较大时采用基于干扰观测器的滤波方法进行抗干扰卡尔曼滤波以抑制干扰影响，保证滤波系统的性能。

附图说明

图1为本发明所述的无人系统自控状态下滤波切换控制单元和方法原理框图；

图2为本发明所述的无人系统自控状态下滤波切换控制方法流程图。

具体实施方式

如图1所示，本发明所述的无人系统自控状态下滤波切换控制单元和方法原理框图，其主要包括标准卡尔曼滤波单元、抗干扰卡尔曼滤波单元和切换控制单元。在无人系统进行多传感器组合导航滤波解算时，滤波系统接收传感器量测信息，按照当前工作模式，通过相应的卡尔曼滤波算法得到系统状态的最优估计值。滤波模式的切换由切换控制单元完成，切换控制单元的输入信号为抗干扰卡尔曼滤波单元输出的干扰估计值和标准卡尔曼滤波单元输出的新息向量，输出信号为模式切换指令，所实现的目的是当干扰较大时，使系统工作在抗干扰卡尔曼滤波模式以保证滤波精度；当干扰较小时，使系统工作在标准卡尔曼滤波模式以降低计算量减小计算误差的累积。

如图2所示，本发明所述的无人系统自控状态下滤波切换控制方法流程如下，以下描述的无人系统以无人机系统为具体应用对象。

(1)根据无人机所用滤波系统所受外部干扰特性，建立基于外部干扰的离散状态空间模型；

通常情况下，可将无人机所采用的滤波系统受到的外部干扰视为高斯白噪声，在其作用下线性系统的输出变量可由如下形式的状态空间模型来描述：

其中w(k)表示k时刻干扰子系统的状态变量；δ(k)表示k时刻干扰子系统输入，为高斯零均值白噪声；d(k)为k时刻滤波系统所受的干扰；W，H，V为已知的系数矩阵。

(2)将第一步建立的基于外部干扰的离散状态空间模型与原系统模型联立，得到状态增广系统；

假设所要估计的原系统状态可由如下线性离散状态空间模型来描述：

其中x(k)表示k时刻被估计的系统状态；d(k)为k时刻滤波系统所受的干扰；y(k)表示k时刻系统输出量测值；F，G，M为已知的系数矩阵；ω(k)，υ(k)为高斯零均值不相关白噪声，其统计特性(均值)满足：

E{ω(k)ω^T(j)}＝Q_kδ_k-j

E{υ(k)υ^T(j)}＝R_kδ_k-j

E{ω(k)υ^T(j)}＝0

其中Q_k，R_k分别表示k时刻过程噪声和量测噪声的协方差矩阵；δ_k-j是克罗内克Kronecker-δ函数，即如果k＝j则δ_k-j＝1，否则δ_k-j＝0。将干扰模型与原系统模型联立，得到以下状态增广系统：

其中为k时刻的增广状态；y(k)表示k时刻状态增广系统输出量测值；为高斯零均值不相关白噪声；

为已知的系数矩阵，I为单位阵。

(3)根据第二步得到的状态增广系统，设计标准卡尔曼滤波器估计其状态；

第二步得到的状态增广系统的形式与标准卡尔曼滤波模式下系统的形式一致，不同之处仅仅在于增广状态变量中加入了干扰子系统的状态，因此维数有所增加，但针对状态增广系统的卡尔曼滤波器设计步骤与形式均同标准卡尔曼滤波模式下一致，可由如下递推形式给出：

其中为k时刻先验估计误差协方差阵；为k时刻估计误差协方差阵；为k-1时刻新息向量；K_k为k时刻卡尔曼滤波增益阵。得到的估计值之后，再由和就可以同时估计出外部干扰和原系统状态。

(4)根据标准卡尔曼滤波单元输出和抗干扰卡尔曼滤波单元输出与对应阈值的比较结果，判断干扰作用大小，决定切换滤波模式；

将标准卡尔曼滤波单元输出的新息向量的2-范数||Δ_k||₂和抗干扰卡尔曼滤波单元输出的干扰估计值的2-范数送入切换控制单元，按以下方式进行判断：

1)当时，说明系统所受干扰作用较小，切换控制单元输出的选通信号σ选通标准卡尔曼滤波单元，将滤波系统工作模式切换至标准卡尔曼滤波模式；

2)当时，说明系统所受干扰作用较大，切换控制单元输出的选通信号σ选通抗干扰卡尔曼滤波单元，将滤波系统工作模式切换至抗干扰卡尔曼滤波模式。

需要说明的是，阈值和可根据导航计算机的精度高低进行调整。具体地，当计算机精度较高，系统计算误差和功耗均相对较小时，可适当减小和使系统更多地工作在抗干扰卡尔曼滤波模式下；当计算机精度较低，系统计算误差或功耗相对较大时，可适当增大和使系统更多地工作在标准卡尔曼滤波模式下。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种基于无人系统自控状态下滤波切换控制单元的控制方法，其特征在于：所述滤波切换控制单元包括标准卡尔曼滤波单元、抗干扰卡尔曼滤波单元以及切换控制单元，所述滤波切换控制方法包括以下步骤：

(1)根据无人系统所采用的滤波系统所受外部干扰特性，建立基于外部干扰的离散状态空间模型为：

其中w(k)表示k时刻干扰子系统的状态变量；δ(k)表示k时刻干扰子系统输入，为高斯零均值白噪声；d(k)为k时刻滤波系统所受的干扰；W，H，V为已知的系数矩阵；

(2)将步骤(1)建立的基于外部干扰的离散状态空间模型与原系统模型联立，得到状态增广系统为：

其中为k时刻的增广状态；y(k)表示k时刻状态增广系统输出量测值；ω(k)，υ(k)为高斯零均值不相关白噪声；

C＝[M 0]，F，G，M为已知的系数矩阵；

(3)根据步骤(2)得到的状态增广系统，设计标准卡尔曼滤波器并估计其状态；

针对状态增广系统的卡尔曼滤波器设计由如下递推形式给出：

其中为k时刻先验估计误差协方差阵；为k时刻估计误差协方差阵；Q_k，R_k分别表示k时刻过程噪声和量测噪声的协方差矩阵，为k-1时刻新息向量；K_k为k时刻卡尔曼滤波增益阵；得到的估计值之后，再由和就可以同时估计出外部干扰和原系统状态；

(4)根据标准卡尔曼滤波单元输出和抗干扰卡尔曼滤波单元输出与对应阈值的比较结果，判断干扰作用大小，决定切换滤波模式，从而进行反馈控制；

将标准卡尔曼滤波单元输出的新息向量的2-范数||Δ_k||₂和抗干扰卡尔曼滤波单元输出的干扰估计值的2-范数送入切换控制单元，按以下方式进行判断：

1)当时，采用标准卡尔曼滤波模式；

2)当时，采用抗干扰卡尔曼滤波模式；

其中，和分别指的是新息向量的阈值以及干扰估计值的阈值。